Ensure rclcpp::TimerBase thread-safety.

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

rclcpp/CHANGELOG.rst

@@ -2,6 +2,80 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.8.4 (2020-01-17)
+------------------
+* Intra-process subscriber should use RMW actual qos (ros2`#913 <https://github.com/ros2/rclcpp/issues/913>`_) (`#914 <https://github.com/ros2/rclcpp/issues/914>`_) (`#965 <https://github.com/ros2/rclcpp/issues/965>`_)
+* Contributors: Todd Malsbary
+
+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>`_)

rclcpp/CMakeLists.txt

@@ -7,12 +7,14 @@ find_package(builtin_interfaces REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rcl_yaml_param_parser REQUIRED)
+find_package(rcpputils REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
 find_package(rosgraph_msgs REQUIRED)
 find_package(rosidl_generator_cpp REQUIRED)
 find_package(rosidl_typesupport_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
+find_package(tracetools REQUIRED)
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
@@ -31,6 +33,9 @@ set(${PROJECT_NAME}_SRCS
   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/duration.cpp
   src/rclcpp/event.cpp
   src/rclcpp/exceptions.cpp
@@ -42,7 +47,6 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/graph_listener.cpp
   src/rclcpp/init_options.cpp
   src/rclcpp/intra_process_manager.cpp
-  src/rclcpp/intra_process_manager_impl.cpp
   src/rclcpp/logger.cpp
   src/rclcpp/memory_strategies.cpp
   src/rclcpp/memory_strategy.cpp
@@ -70,6 +74,7 @@ set(${PROJECT_NAME}_SRCS
   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
@@ -106,10 +111,13 @@ add_library(${PROJECT_NAME}
 ament_target_dependencies(${PROJECT_NAME}
   "rcl"
   "rcl_yaml_param_parser"
+  "rcpputils"
   "builtin_interfaces"
   "rosgraph_msgs"
   "rosidl_typesupport_cpp"
-  "rosidl_generator_cpp")
+  "rosidl_generator_cpp"
+  "tracetools"
+)
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
@@ -129,12 +137,14 @@ ament_export_libraries(${PROJECT_NAME})
 
 ament_export_dependencies(ament_cmake)
 ament_export_dependencies(rcl)
+ament_export_dependencies(rcpputils)
 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_generator_cpp)
 ament_export_dependencies(rcl_yaml_param_parser)
+ament_export_dependencies(tracetools)
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
@@ -143,8 +153,12 @@ if(BUILD_TESTING)
 
   find_package(rmw_implementation_cmake REQUIRED)
 
+  find_package(test_msgs REQUIRED)
+
   include(cmake/rclcpp_add_build_failure_test.cmake)
 
+  add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_BINARY_DIR}/test/resources")
+
   ament_add_gtest(test_client test/test_client.cpp)
   if(TARGET test_client)
     ament_target_dependencies(test_client
@@ -155,6 +169,18 @@ if(BUILD_TESTING)
     )
     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_generator_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
@@ -174,17 +200,7 @@ if(BUILD_TESTING)
       "rosidl_typesupport_cpp"
     )
   endif()
-  ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp)
-  if(TARGET test_mapped_ring_buffer)
-    ament_target_dependencies(test_mapped_ring_buffer
-      "rcl"
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_generator_cpp"
-      "rosidl_typesupport_cpp"
-    )
-  endif()
-  ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp)
+  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"
@@ -193,11 +209,40 @@ if(BUILD_TESTING)
       "rosidl_generator_cpp"
       "rosidl_typesupport_cpp"
     )
+    target_link_libraries(test_intra_process_manager ${PROJECT_NAME})
   endif()
-  ament_add_gtest(test_node test/test_node.cpp)
+  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_generator_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_generator_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_generator_cpp"
       "rosidl_typesupport_cpp"
@@ -242,6 +287,11 @@ if(BUILD_TESTING)
     )
     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
@@ -344,7 +394,6 @@ if(BUILD_TESTING)
     )
     target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME})
   endif()
-  find_package(test_msgs REQUIRED)
   ament_add_gtest(test_subscription_traits test/test_subscription_traits.cpp)
   if(TARGET test_subscription_traits)
     ament_target_dependencies(test_subscription_traits
@@ -360,38 +409,17 @@ if(BUILD_TESTING)
     target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
   endif()
 
-  get_default_rmw_implementation(default_rmw)
-  find_package(${default_rmw} REQUIRED)
-  get_rmw_typesupport(typesupport_impls_cpp "${default_rmw}" LANGUAGE "cpp")
-  get_rmw_typesupport(typesupport_impls_c "${default_rmw}" LANGUAGE "c")
-  set(mock_msg_files
-    "test/mock_msgs/srv/Mock.srv")
-  rosidl_generate_interfaces(mock_msgs
-    ${mock_msg_files}
-    LIBRARY_NAME "rclcpp"
-    SKIP_INSTALL)
-
   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
-    APPEND_LIBRARY_DIRS "${append_library_dirs}")
-  if(TARGET test_externally_defined_services)
-    ament_target_dependencies(test_externally_defined_services
-      "rcl"
-    )
-    target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
-    foreach(typesupport_impl_cpp ${typesupport_impls_cpp})
-      rosidl_target_interfaces(test_externally_defined_services
-        mock_msgs ${typesupport_impl_cpp})
-    endforeach()
-    foreach(typesupport_impl_c ${typesupport_impls_c})
-      rosidl_target_interfaces(test_externally_defined_services
-        mock_msgs ${typesupport_impl_c})
-    endforeach()
-  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}")
@@ -463,16 +491,10 @@ if(BUILD_TESTING)
     target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
   endif()
 
-  ament_add_gtest(test_local_parameters test/test_local_parameters.cpp)
-  if(TARGET test_local_parameters)
-    ament_target_dependencies(test_local_parameters
-      "rcl_interfaces"
-      "rmw"
-      "rosidl_generator_cpp"
-      "rosidl_typesupport_cpp"
-    )
-    target_link_libraries(test_local_parameters ${PROJECT_NAME})
-  endif()
+  # Install test resources
+  install(
+    DIRECTORY test/resources
+    DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test)
 endif()
 
 ament_package()

rclcpp/include/rclcpp/any_executable.hpp

@@ -42,7 +42,6 @@ struct AnyExecutable
 
   // Only one of the following pointers will be set.
   rclcpp::SubscriptionBase::SharedPtr subscription;
-  rclcpp::SubscriptionBase::SharedPtr subscription_intra_process;
   rclcpp::TimerBase::SharedPtr timer;
   rclcpp::ServiceBase::SharedPtr service;
   rclcpp::ClientBase::SharedPtr client;

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -23,6 +23,7 @@
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/types.h"
+#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -86,6 +87,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,6 +96,7 @@ public:
     } else {
       throw std::runtime_error("unexpected request without any callback set");
     }
+    TRACEPOINT(callback_end, (const void *)this);
   }
 };
 

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -26,6 +26,8 @@
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "tracetools/tracetools.h"
+#include "tracetools/utils.hpp"
 
 namespace rclcpp
 {
@@ -155,6 +157,7 @@ public:
   void dispatch(
     std::shared_ptr<MessageT> message, const rmw_message_info_t & 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,30 +177,36 @@ public:
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }
+    TRACEPOINT(callback_end, (const void *)this);
   }
 
   void dispatch_intra_process(
     ConstMessageSharedPtr message, const rmw_message_info_t & 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_ ||
+      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 "
+        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 rmw_message_info_t & message_info)
   {
+    TRACEPOINT(callback_start, (const void *)this, true);
     if (shared_ptr_callback_) {
       typename std::shared_ptr<MessageT> shared_message = std::move(message);
       shared_ptr_callback_(shared_message);
@@ -209,18 +218,45 @@ public:
     } 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"
+      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()
+  bool use_take_shared_method() const
   {
     return const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_;
   }
 
+  void register_callback_for_tracing()
+  {
+    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_));
+    }
+  }
+
 private:
   std::shared_ptr<MessageAlloc> message_allocator_;
   MessageDeleter message_deleter_;

rclcpp/include/rclcpp/callback_group.hpp

@@ -21,9 +21,9 @@
 #include <vector>
 
 #include "rclcpp/client.hpp"
-#include "rclcpp/publisher.hpp"
+#include "rclcpp/publisher_base.hpp"
 #include "rclcpp/service.hpp"
-#include "rclcpp/subscription.hpp"
+#include "rclcpp/subscription_base.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
@@ -62,25 +62,40 @@ public:
   RCLCPP_PUBLIC
   explicit CallbackGroup(CallbackGroupType group_type);
 
-  RCLCPP_PUBLIC
-  const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
-  get_subscription_ptrs() const;
+  template<typename Function>
+  rclcpp::SubscriptionBase::SharedPtr
+  find_subscription_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::SubscriptionBase, Function>(func, subscription_ptrs_);
+  }
 
-  RCLCPP_PUBLIC
-  const std::vector<rclcpp::TimerBase::WeakPtr> &
-  get_timer_ptrs() const;
+  template<typename Function>
+  rclcpp::TimerBase::SharedPtr
+  find_timer_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::TimerBase, Function>(func, timer_ptrs_);
+  }
 
-  RCLCPP_PUBLIC
-  const std::vector<rclcpp::ServiceBase::WeakPtr> &
-  get_service_ptrs() const;
+  template<typename Function>
+  rclcpp::ServiceBase::SharedPtr
+  find_service_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::ServiceBase, Function>(func, service_ptrs_);
+  }
 
-  RCLCPP_PUBLIC
-  const std::vector<rclcpp::ClientBase::WeakPtr> &
-  get_client_ptrs() const;
+  template<typename Function>
+  rclcpp::ClientBase::SharedPtr
+  find_client_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::ClientBase, Function>(func, client_ptrs_);
+  }
 
-  RCLCPP_PUBLIC
-  const std::vector<rclcpp::Waitable::WeakPtr> &
-  get_waitable_ptrs() const;
+  template<typename Function>
+  rclcpp::Waitable::SharedPtr
+  find_waitable_ptrs_if(Function func) const
+  {
+    return _find_ptrs_if_impl<rclcpp::Waitable, Function>(func, waitable_ptrs_);
+  }
 
   RCLCPP_PUBLIC
   std::atomic_bool &
@@ -130,6 +145,21 @@ protected:
   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

rclcpp/include/rclcpp/clock.hpp

@@ -16,12 +16,16 @@
 #define RCLCPP__CLOCK_HPP_
 
 #include <functional>
+#include <thread>
+#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
 {
@@ -93,6 +97,12 @@ public:
   rcl_clock_type_t
   get_clock_type() const noexcept;
 
+  RCLCPP_PUBLIC
+  std::mutex &
+  get_clock_mutex() noexcept {
+    return clock_mutex_;
+  }
+
   // Add a callback to invoke if the jump threshold is exceeded.
   /**
    * These callback functions must remain valid as long as the
@@ -134,6 +144,7 @@ private:
   rcl_clock_t rcl_clock_;
   friend TimeSource;  /// Allow TimeSource to access the rcl_clock_ datatype.
   rcl_allocator_t allocator_;
+  std::mutex clock_mutex_;
 };
 
 }  // namespace rclcpp

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::callback_group::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

@@ -29,36 +29,6 @@
 namespace rclcpp
 {
 
-template<
-  typename MessageT,
-  typename AllocatorT = std::allocator<void>,
-  typename PublisherT = ::rclcpp::Publisher<MessageT, AllocatorT>>
-// cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function
-[[deprecated("use alternative rclcpp::create_publisher() signatures")]]
-std::shared_ptr<PublisherT>
-create_publisher(
-  rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
-  const std::string & topic_name,
-  const rmw_qos_profile_t & qos_profile,
-  const PublisherEventCallbacks & event_callbacks,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group,
-  bool use_intra_process_comms,
-  std::shared_ptr<AllocatorT> allocator)
-{
-  auto publisher_options = rcl_publisher_get_default_options();
-  publisher_options.qos = qos_profile;
-
-  auto pub = node_topics->create_publisher(
-    topic_name,
-    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(event_callbacks, allocator),
-    publisher_options,
-    use_intra_process_comms);
-
-  node_topics->add_publisher(pub, group);
-
-  return std::dynamic_pointer_cast<PublisherT>(pub);
-}
-
 /// Create and return a publisher of the given MessageT type.
 /**
  * The NodeT type only needs to have a method called get_node_topics_interface()
@@ -67,7 +37,7 @@ create_publisher(
 template<
   typename MessageT,
   typename AllocatorT = std::allocator<void>,
-  typename PublisherT = ::rclcpp::Publisher<MessageT, AllocatorT>,
+  typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>,
   typename NodeT>
 std::shared_ptr<PublisherT>
 create_publisher(
@@ -76,43 +46,23 @@ create_publisher(
   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);
 
-  std::shared_ptr<AllocatorT> allocator = options.allocator;
-  if (!allocator) {
-    allocator = std::make_shared<AllocatorT>();
-  }
-
-  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_topics->get_node_base_interface()->get_use_intra_process_default();
-      break;
-    default:
-      throw std::runtime_error("Unrecognized IntraProcessSetting value");
-      break;
-  }
-
-  // TODO(wjwwood): convert all of the interfaces to use QoS and PublisherOptionsBase
+  // Create the publisher.
   auto pub = node_topics->create_publisher(
     topic_name,
-    rclcpp::create_publisher_factory<MessageT, AllocatorT, PublisherT>(
-      options.event_callbacks,
-      allocator
-    ),
-    options.template to_rcl_publisher_options<MessageT>(qos),
-    use_intra_process
+    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);
 }
 

rclcpp/include/rclcpp/create_subscription.hpp

@@ -29,49 +29,6 @@
 namespace rclcpp
 {
 
-template<
-  typename MessageT,
-  typename CallbackT,
-  typename AllocatorT,
-  typename CallbackMessageT,
-  typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>>
-// cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function
-[[deprecated("use alternative rclcpp::create_subscription() signatures")]]
-typename std::shared_ptr<SubscriptionT>
-create_subscription(
-  rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
-  const std::string & topic_name,
-  CallbackT && callback,
-  const rmw_qos_profile_t & qos_profile,
-  const SubscriptionEventCallbacks & event_callbacks,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group,
-  bool ignore_local_publications,
-  bool use_intra_process_comms,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-    CallbackMessageT, AllocatorT>::SharedPtr
-  msg_mem_strat,
-  typename std::shared_ptr<AllocatorT> allocator)
-{
-  auto subscription_options = rcl_subscription_get_default_options();
-  subscription_options.qos = qos_profile;
-  subscription_options.ignore_local_publications = ignore_local_publications;
-
-  auto factory = rclcpp::create_subscription_factory
-    <MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>(
-    std::forward<CallbackT>(callback),
-    event_callbacks,
-    msg_mem_strat,
-    allocator);
-
-  auto sub = node_topics->create_subscription(
-    topic_name,
-    factory,
-    subscription_options,
-    use_intra_process_comms);
-  node_topics->add_subscription(sub, group);
-  return std::dynamic_pointer_cast<SubscriptionT>(sub);
-}
-
 /// Create and return a subscription of the given MessageT type.
 /**
  * The NodeT type only needs to have a method called get_node_topics_interface()
@@ -85,6 +42,10 @@ template<
   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(
@@ -95,49 +56,23 @@ create_subscription(
   const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
     rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
   ),
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-    CallbackMessageT, AllocatorT>::SharedPtr
-  msg_mem_strat = nullptr)
+  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));
 
-  if (!msg_mem_strat) {
-    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
-    msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, AllocatorT>::create_default();
-  }
+  auto factory = rclcpp::create_subscription_factory<MessageT>(
+    std::forward<CallbackT>(callback),
+    options,
+    msg_mem_strat
+  );
 
-  std::shared_ptr<AllocatorT> allocator = options.allocator;
-  if (!allocator) {
-    allocator = std::make_shared<AllocatorT>();
-  }
-  auto factory = rclcpp::create_subscription_factory
-    <MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>(
-    std::forward<CallbackT>(callback), options.event_callbacks, msg_mem_strat, allocator);
-
-  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_topics->get_node_base_interface()->get_use_intra_process_default();
-      break;
-    default:
-      throw std::runtime_error("Unrecognized IntraProcessSetting value");
-      break;
-  }
-
-  // TODO(wjwwood): convert all of the interfaces to use QoS and SubscriptionOptionsBase
-  auto sub = node_topics->create_subscription(
-    topic_name,
-    factory,
-    options.template to_rcl_subscription_options<MessageT>(qos),
-    use_intra_process);
+  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);
 }
 

rclcpp/include/rclcpp/create_timer.hpp

@@ -0,0 +1,73 @@
+// 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 <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(
+  node_interfaces::NodeBaseInterface * node_base,
+  node_interfaces::NodeTimersInterface * node_timers,
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Duration period,
+  CallbackT && callback,
+  rclcpp::callback_group::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::callback_group::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);
+}
+
+}  // 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_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/duration.hpp

@@ -28,10 +28,12 @@ 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
@@ -94,6 +96,10 @@ public:
   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

rclcpp/include/rclcpp/exceptions.hpp

@@ -17,11 +17,14 @@
 
 #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
@@ -167,6 +170,31 @@ 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
 {
@@ -219,6 +247,13 @@ class ParameterImmutableException : public std::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
 

rclcpp/include/rclcpp/executor.hpp

@@ -22,6 +22,7 @@
 #include <iostream>
 #include <list>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
@@ -47,6 +48,7 @@ namespace executor
 /// 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.
  */
@@ -210,8 +212,8 @@ public:
 
   /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
   /**
-   * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this
-   *   function.
+   * \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
@@ -303,11 +305,6 @@ protected:
   execute_subscription(
     rclcpp::SubscriptionBase::SharedPtr subscription);
 
-  RCLCPP_PUBLIC
-  static void
-  execute_intra_process_subscription(
-    rclcpp::SubscriptionBase::SharedPtr subscription);
-
   RCLCPP_PUBLIC
   static void
   execute_timer(rclcpp::TimerBase::SharedPtr timer);
@@ -332,10 +329,6 @@ protected:
   rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_timer(rclcpp::TimerBase::SharedPtr timer);
 
-  RCLCPP_PUBLIC
-  void
-  get_next_timer(AnyExecutable & any_exec);
-
   RCLCPP_PUBLIC
   bool
   get_next_ready_executable(AnyExecutable & any_executable);
@@ -355,6 +348,9 @@ protected:
   /// 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_;
 

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 
+#include <chrono>
 #include <memory>
 #include <mutex>
 #include <set>
@@ -53,7 +54,8 @@ public:
   MultiThreadedExecutor(
     const executor::ExecutorArgs & args = executor::ExecutorArgs(),
     size_t number_of_threads = 0,
-    bool yield_before_execute = false);
+    bool yield_before_execute = false,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
   RCLCPP_PUBLIC
   virtual ~MultiThreadedExecutor();
@@ -77,6 +79,7 @@ private:
   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_;
 };

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/intra_process_manager.hpp

@@ -0,0 +1,426 @@
+// 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,173 @@
+// 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.
+    any_callback_.register_callback_for_tracing();
+  }
+
+  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.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

@@ -96,6 +96,24 @@ struct function_traits<
   : 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(__cdecl 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)

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,420 +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 <mutex>
-#include <unordered_map>
-#include <utility>
-#include <set>
-
-#include "rclcpp/allocator/allocator_deleter.hpp"
-#include "rclcpp/intra_process_manager_impl.hpp"
-#include "rclcpp/mapped_ring_buffer.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/publisher_base.hpp"
-#include "rclcpp/subscription_base.hpp"
-#include "rclcpp/visibility_control.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)
-
-  RCLCPP_PUBLIC
-  explicit IntraProcessManager(
-    IntraProcessManagerImplBase::SharedPtr state = create_default_impl());
-
-  RCLCPP_PUBLIC
-  virtual ~IntraProcessManager();
-
-  /// 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.
-   */
-  RCLCPP_PUBLIC
-  uint64_t
-  add_subscription(SubscriptionBase::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.
-  /**
-   * 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.
-   */
-  RCLCPP_PUBLIC
-  uint64_t
-  add_publisher(
-    rclcpp::PublisherBase::SharedPtr publisher,
-    size_t buffer_size = 0);
-
-  /// 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);
-
-  /// 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>>
-  uint64_t
-  store_intra_process_message(
-    uint64_t intra_process_publisher_id,
-    std::shared_ptr<const MessageT> 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 = impl_->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.
-
-    impl_->store_intra_process_message(intra_process_publisher_id, message_seq);
-
-    // Return the message sequence which is sent to the subscription.
-    return message_seq;
-  }
-
-  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 = impl_->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, std::move(message));
-    // TODO(wjwwood): do something when a message was displaced. log debug?
-    (void)did_replace;  // Avoid unused variable warning.
-
-    impl_->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;
-    std::lock_guard<std::mutex> lock(take_mutex_);
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->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(message_sequence_number, message);
-    } else {
-      // This is the last one to be returned, transfer ownership.
-      typed_buffer->pop(message_sequence_number, message);
-    }
-  }
-
-  template<
-    typename MessageT, typename Alloc = std::allocator<void>>
-  void
-  take_intra_process_message(
-    uint64_t intra_process_publisher_id,
-    uint64_t message_sequence_number,
-    uint64_t requesting_subscriptions_intra_process_id,
-    std::shared_ptr<const MessageT> & 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;
-    std::lock_guard<std::mutex> lock(take_mutex_);
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->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(message_sequence_number, message);
-    } else {
-      // This is the last one to be returned, transfer ownership.
-      typed_buffer->pop(message_sequence_number, message);
-    }
-  }
-
-  /// 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 to a topic, given the publisher id.
-  RCLCPP_PUBLIC
-  size_t
-  get_subscription_count(uint64_t intra_process_publisher_id) const;
-
-private:
-  RCLCPP_PUBLIC
-  static uint64_t
-  get_next_unique_id();
-
-  IntraProcessManagerImplBase::SharedPtr impl_;
-  std::mutex take_mutex_;
-};
-
-}  // namespace intra_process_manager
-}  // namespace rclcpp
-
-#endif  // RCLCPP__INTRA_PROCESS_MANAGER_HPP_

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

@@ -1,358 +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_IMPL_HPP_
-#define RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_
-
-#include <algorithm>
-#include <array>
-#include <atomic>
-#include <cstring>
-#include <functional>
-#include <limits>
-#include <map>
-#include <memory>
-#include <mutex>
-#include <set>
-#include <stdexcept>
-#include <string>
-#include <unordered_map>
-#include <utility>
-
-#include "rmw/validate_full_topic_name.h"
-
-#include "rclcpp/macros.hpp"
-#include "rclcpp/mapped_ring_buffer.hpp"
-#include "rclcpp/publisher_base.hpp"
-#include "rclcpp/subscription_base.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace intra_process_manager
-{
-
-class IntraProcessManagerImplBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerImplBase)
-
-  IntraProcessManagerImplBase() = default;
-  virtual ~IntraProcessManagerImplBase() = default;
-
-  virtual void
-  add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) = 0;
-
-  virtual void
-  remove_subscription(uint64_t intra_process_subscription_id) = 0;
-
-  virtual void add_publisher(
-    uint64_t id,
-    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;
-
-  virtual size_t
-  get_subscription_count(uint64_t intra_process_publisher_id) const = 0;
-
-private:
-  RCLCPP_DISABLE_COPY(IntraProcessManagerImplBase)
-};
-
-template<typename Allocator = std::allocator<void>>
-class IntraProcessManagerImpl : public IntraProcessManagerImplBase
-{
-public:
-  IntraProcessManagerImpl() = default;
-  ~IntraProcessManagerImpl() = default;
-
-  void
-  add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription)
-  {
-    subscriptions_[id] = subscription;
-    subscription_ids_by_topic_[fixed_size_string(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,
-    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)
-  {
-    std::lock_guard<std::mutex> lock(runtime_mutex_);
-    auto it = publishers_.find(intra_process_publisher_id);
-    if (it == publishers_.end()) {
-      throw std::runtime_error("get_publisher_info_for_id called with invalid publisher id");
-    }
-    PublisherInfo & info = it->second;
-    // Calculate the next message sequence number.
-    message_seq = info.sequence_number.fetch_add(1);
-
-    return info.buffer;
-  }
-
-  void
-  store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq)
-  {
-    std::lock_guard<std::mutex> lock(runtime_mutex_);
-    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_[fixed_size_string(publisher->get_topic_name())];
-    // Store the list for later comparison.
-    if (info.target_subscriptions_by_message_sequence.count(message_seq) == 0) {
-      info.target_subscriptions_by_message_sequence.emplace(
-        message_seq, AllocSet(std::less<uint64_t>(), uint64_allocator));
-    } else {
-      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
-  )
-  {
-    std::lock_guard<std::mutex> lock(runtime_mutex_);
-    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;
-  }
-
-  size_t
-  get_subscription_count(uint64_t intra_process_publisher_id) const
-  {
-    auto publisher_it = publishers_.find(intra_process_publisher_id);
-    if (publisher_it == publishers_.end()) {
-      // Publisher is either invalid or no longer exists.
-      return 0;
-    }
-    auto publisher = publisher_it->second.publisher.lock();
-    if (!publisher) {
-      throw std::runtime_error("publisher has unexpectedly gone out of scope");
-    }
-    auto sub_map_it =
-      subscription_ids_by_topic_.find(fixed_size_string(publisher->get_topic_name()));
-    if (sub_map_it == subscription_ids_by_topic_.end()) {
-      // No intraprocess subscribers
-      return 0;
-    }
-    return sub_map_it->second.size();
-  }
-
-private:
-  RCLCPP_DISABLE_COPY(IntraProcessManagerImpl)
-
-  using FixedSizeString = std::array<char, RMW_TOPIC_MAX_NAME_LENGTH + 1>;
-
-  FixedSizeString
-  fixed_size_string(const char * str) const
-  {
-    FixedSizeString ret;
-    size_t size = std::strlen(str) + 1;
-    if (size > ret.size()) {
-      throw std::runtime_error("failed to copy topic name");
-    }
-    std::memcpy(ret.data(), str, size);
-    return ret;
-  }
-  struct strcmp_wrapper
-  {
-    bool
-    operator()(const FixedSizeString lhs, const FixedSizeString rhs) const
-    {
-      return std::strcmp(lhs.data(), rhs.data()) < 0;
-    }
-  };
-
-  template<typename T>
-  using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
-
-  RebindAlloc<uint64_t> uint64_allocator;
-
-  using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
-  using SubscriptionMap = std::unordered_map<
-    uint64_t, SubscriptionBase::WeakPtr,
-    std::hash<uint64_t>, std::equal_to<uint64_t>,
-    RebindAlloc<std::pair<const uint64_t, SubscriptionBase::WeakPtr>>>;
-
-  using IDTopicMap = std::map<
-    FixedSizeString,
-    AllocSet,
-    strcmp_wrapper,
-    RebindAlloc<std::pair<const FixedSizeString, AllocSet>>>;
-
-  SubscriptionMap subscriptions_;
-
-  IDTopicMap subscription_ids_by_topic_;
-
-  struct PublisherInfo
-  {
-    RCLCPP_DISABLE_COPY(PublisherInfo)
-
-    PublisherInfo() = default;
-
-    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_;
-
-  std::mutex runtime_mutex_;
-};
-
-RCLCPP_PUBLIC
-IntraProcessManagerImplBase::SharedPtr
-create_default_impl();
-
-}  // namespace intra_process_manager
-}  // namespace rclcpp
-
-#endif  // RCLCPP__INTRA_PROCESS_MANAGER_IMPL_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/macros.hpp

@@ -66,6 +66,7 @@
 #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(...) \

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -1,319 +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 <mutex>
-#include <stdexcept>
-#include <utility>
-#include <vector>
-
-#include "rclcpp/allocator/allocator_common.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-namespace mapped_ring_buffer
-{
-
-class RCLCPP_PUBLIC MappedRingBufferBase
-{
-public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase)
-};
-
-/// Ring buffer container of shared_ptr's or 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 ConstElemSharedPtr = std::shared_ptr<const 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.
-   * \param allocator optional custom allocator
-   */
-  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(uint64_t key, ElemUniquePtr & value)
-  {
-    std::lock_guard<std::mutex> lock(data_mutex_);
-    auto it = get_iterator_of_key(key);
-    value = nullptr;
-    if (it != elements_.end() && it->in_use) {
-      if (it->unique_value) {
-        ElemDeleter deleter = it->unique_value.get_deleter();
-        auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
-        ElemAllocTraits::construct(*allocator_.get(), ptr, *it->unique_value);
-        value = ElemUniquePtr(ptr, deleter);
-      } else if (it->shared_value) {
-        ElemDeleter * deleter = std::get_deleter<ElemDeleter, const T>(it->shared_value);
-        auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
-        ElemAllocTraits::construct(*allocator_.get(), ptr, *it->shared_value);
-        if (deleter) {
-          value = ElemUniquePtr(ptr, *deleter);
-        } else {
-          value = ElemUniquePtr(ptr);
-        }
-      } else {
-        throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
-      }
-    }
-  }
-
-  /// Share 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
-  get(uint64_t key, ConstElemSharedPtr & value)
-  {
-    std::lock_guard<std::mutex> lock(data_mutex_);
-    auto it = get_iterator_of_key(key);
-    value.reset();
-    if (it != elements_.end() && it->in_use) {
-      if (!it->shared_value) {
-        // The stored unique_ptr is upgraded to a shared_ptr here.
-        // All the remaining get and pop calls done with unique_ptr
-        // signature will receive a copy.
-        if (!it->unique_value) {
-          throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
-        }
-        it->shared_value = std::move(it->unique_value);
-      }
-      value = it->shared_value;
-    }
-  }
-
-  /// Give the ownership of the stored value to the caller if possible, or copy and release.
-  /**
-   * The key is matched if an element in the ring buffer has a matching key.
-   * This method may allocate in order to return a copy.
-   *
-   * If the stored value is a shared_ptr, it is not possible to downgrade it to a unique_ptr.
-   * In that case, a copy is returned and the stored value is released.
-   *
-   * 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(uint64_t key, ElemUniquePtr & value)
-  {
-    std::lock_guard<std::mutex> lock(data_mutex_);
-    auto it = get_iterator_of_key(key);
-    value = nullptr;
-    if (it != elements_.end() && it->in_use) {
-      if (it->unique_value) {
-        value = std::move(it->unique_value);
-      } else if (it->shared_value) {
-        auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
-        ElemAllocTraits::construct(*allocator_.get(), ptr, *it->shared_value);
-        auto deleter = std::get_deleter<ElemDeleter, const T>(it->shared_value);
-        if (deleter) {
-          value = ElemUniquePtr(ptr, *deleter);
-        } else {
-          value = ElemUniquePtr(ptr);
-        }
-        it->shared_value.reset();
-      } else {
-        throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
-      }
-      it->in_use = false;
-    }
-  }
-
-  /// Give the ownership of the stored value to the caller, 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(uint64_t key, ConstElemSharedPtr & value)
-  {
-    std::lock_guard<std::mutex> lock(data_mutex_);
-    auto it = get_iterator_of_key(key);
-    if (it != elements_.end() && it->in_use) {
-      if (it->shared_value) {
-        value = std::move(it->shared_value);
-      } else if (it->unique_value) {
-        value = std::move(it->unique_value);
-      } else {
-        throw std::runtime_error("Unexpected empty MappedRingBuffer element.");
-      }
-      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 the value will be a nullptr.
-   * If a pair were replaced, its smart pointer is reset.
-   *
-   * \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, ConstElemSharedPtr value)
-  {
-    std::lock_guard<std::mutex> lock(data_mutex_);
-    bool did_replace = elements_[head_].in_use;
-    Element & element = elements_[head_];
-    element.key = key;
-    element.unique_value.reset();
-    element.shared_value.reset();
-    element.shared_value = value;
-    element.in_use = true;
-    head_ = (head_ + 1) % elements_.size();
-    return did_replace;
-  }
-
-  /// Insert a key-value pair, displacing an existing pair if necessary.
-  /**
-   * See `bool push_and_replace(uint64_t key, const ConstElemSharedPtr & value)`.
-   */
-  bool
-  push_and_replace(uint64_t key, ElemUniquePtr value)
-  {
-    std::lock_guard<std::mutex> lock(data_mutex_);
-    bool did_replace = elements_[head_].in_use;
-    Element & element = elements_[head_];
-    element.key = key;
-    element.unique_value.reset();
-    element.shared_value.reset();
-    element.unique_value = std::move(value);
-    element.in_use = true;
-    head_ = (head_ + 1) % elements_.size();
-    return did_replace;
-  }
-
-  /// Return true if the key is found in the ring buffer, otherwise false.
-  bool
-  has_key(uint64_t key)
-  {
-    std::lock_guard<std::mutex> lock(data_mutex_);
-    return elements_.end() != get_iterator_of_key(key);
-  }
-
-private:
-  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>)
-
-  struct Element
-  {
-    uint64_t key;
-    ElemUniquePtr unique_value;
-    ConstElemSharedPtr shared_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)
-  {
-    auto it = std::find_if(
-      elements_.begin(), elements_.end(),
-      [key](Element & e) -> bool {
-        return e.key == key && e.in_use;
-      });
-    return it;
-  }
-
-  std::vector<Element, VectorAlloc> elements_;
-  size_t head_;
-  std::shared_ptr<ElemAlloc> allocator_;
-  std::mutex data_mutex_;
-};
-
-}  // namespace mapped_ring_buffer
-}  // namespace rclcpp
-
-#endif  // RCLCPP__MAPPED_RING_BUFFER_HPP_

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -79,6 +79,11 @@ public:
     rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeList & weak_nodes) = 0;
 
+  virtual void
+  get_next_timer(
+    rclcpp::executor::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes) = 0;
+
   virtual void
   get_next_waitable(
     rclcpp::executor::AnyExecutable & any_exec,
@@ -102,6 +107,11 @@ public:
     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::callback_group::CallbackGroup::SharedPtr group,
@@ -122,6 +132,11 @@ public:
     rclcpp::ClientBase::SharedPtr client,
     const WeakNodeList & weak_nodes);
 
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_timer(
+    rclcpp::TimerBase::SharedPtr timer,
+    const WeakNodeList & weak_nodes);
+
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_waitable(
     rclcpp::Waitable::SharedPtr waitable,

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -95,16 +95,17 @@ public:
       rclcpp::exceptions::throw_from_rcl_error(ret);
     }
 
-    auto serialized_msg = std::shared_ptr<rcl_serialized_message_t>(msg,
-        [](rmw_serialized_message_t * msg) {
-          auto ret = rmw_serialized_message_fini(msg);
-          delete msg;
-          if (ret != RCL_RET_OK) {
-            RCUTILS_LOG_ERROR_NAMED(
-              "rclcpp",
-              "failed to destroy serialized message: %s", rcl_get_error_string().str);
-          }
-        });
+    auto serialized_msg = std::shared_ptr<rcl_serialized_message_t>(
+      msg,
+      [](rmw_serialized_message_t * msg) {
+        auto fini_ret = rmw_serialized_message_fini(msg);
+        delete msg;
+        if (fini_ret != RCL_RET_OK) {
+          RCUTILS_LOG_ERROR_NAMED(
+            "rclcpp",
+            "failed to destroy serialized message: %s", rcl_get_error_string().str);
+        }
+      });
 
     return serialized_msg;
   }

rclcpp/include/rclcpp/node.hpp

@@ -151,15 +151,17 @@ public:
    *
    * For example, all of these cases will work:
    *
-   *   pub = node->create_publisher<MsgT>("chatter", 10);  // implicitly KeepLast
-   *   pub = node->create_publisher<MsgT>("chatter", QoS(10));  // implicitly KeepLast
-   *   pub = node->create_publisher<MsgT>("chatter", QoS(KeepLast(10)));
-   *   pub = node->create_publisher<MsgT>("chatter", QoS(KeepAll()));
-   *   pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().volatile());
-   *   {
-   *     rclcpp::QoS custom_qos(KeepLast(10), rmw_qos_profile_sensor_data);
-   *     pub = node->create_publisher<MsgT>("chatter", custom_qos);
-   *   }
+   * ```cpp
+   * pub = node->create_publisher<MsgT>("chatter", 10);  // implicitly KeepLast
+   * pub = node->create_publisher<MsgT>("chatter", QoS(10));  // implicitly KeepLast
+   * pub = node->create_publisher<MsgT>("chatter", QoS(KeepLast(10)));
+   * pub = node->create_publisher<MsgT>("chatter", QoS(KeepAll()));
+   * pub = node->create_publisher<MsgT>("chatter", QoS(1).best_effort().volatile());
+   * {
+   *   rclcpp::QoS custom_qos(KeepLast(10), rmw_qos_profile_sensor_data);
+   *   pub = node->create_publisher<MsgT>("chatter", custom_qos);
+   * }
+   * ```
    *
    * The publisher options may optionally be passed as the third argument for
    * any of the above cases.
@@ -172,7 +174,7 @@ public:
   template<
     typename MessageT,
     typename AllocatorT = std::allocator<void>,
-    typename PublisherT = ::rclcpp::Publisher<MessageT, AllocatorT>>
+    typename PublisherT = rclcpp::Publisher<MessageT, AllocatorT>>
   std::shared_ptr<PublisherT>
   create_publisher(
     const std::string & topic_name,
@@ -181,44 +183,6 @@ public:
     PublisherOptionsWithAllocator<AllocatorT>()
   );
 
-  /// Create and return a Publisher.
-  /**
-   * \param[in] topic_name The topic for this publisher to publish on.
-   * \param[in] qos_history_depth The depth of the publisher message queue.
-   * \param[in] allocator Custom allocator.
-   * \return Shared pointer to the created publisher.
-   */
-  template<
-    typename MessageT,
-    typename AllocatorT = std::allocator<void>,
-    typename PublisherT = ::rclcpp::Publisher<MessageT, AllocatorT>>
-  // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function
-  [[deprecated("use create_publisher(const std::string &, const rclcpp::QoS &, ...) instead")]]
-  std::shared_ptr<PublisherT>
-  create_publisher(
-    const std::string & topic_name,
-    size_t qos_history_depth,
-    std::shared_ptr<AllocatorT> allocator);
-
-  /// Create and return a Publisher.
-  /**
-   * \param[in] topic_name The topic for this publisher to publish on.
-   * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation.
-   * \param[in] allocator Optional custom allocator.
-   * \return Shared pointer to the created publisher.
-   */
-  template<
-    typename MessageT,
-    typename AllocatorT = std::allocator<void>,
-    typename PublisherT = ::rclcpp::Publisher<MessageT, AllocatorT>>
-  // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function
-  [[deprecated("use create_publisher(const std::string &, const rclcpp::QoS &, ...) instead")]]
-  std::shared_ptr<PublisherT>
-  create_publisher(
-    const std::string & topic_name,
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
-    std::shared_ptr<AllocatorT> allocator = nullptr);
-
   /// Create and return a Subscription.
   /**
    * \param[in] topic_name The topic to subscribe on.
@@ -228,16 +192,18 @@ public:
    * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
    * \return Shared pointer to the created subscription.
    */
-  /* TODO(jacquelinekay):
-     Windows build breaks when static member function passed as default
-     argument to msg_mem_strat, nullptr is a workaround.
-   */
   template<
     typename MessageT,
     typename CallbackT,
     typename AllocatorT = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<
-      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, AllocatorT>>
+    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
+    >
+  >
   std::shared_ptr<SubscriptionT>
   create_subscription(
     const std::string & topic_name,
@@ -245,84 +211,10 @@ public:
     CallbackT && callback,
     const SubscriptionOptionsWithAllocator<AllocatorT> & options =
     SubscriptionOptionsWithAllocator<AllocatorT>(),
-    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, AllocatorT
-    >::SharedPtr
-    msg_mem_strat = nullptr);
-
-  /// Create and return a Subscription.
-  /**
-   * \param[in] topic_name The topic to subscribe on.
-   * \param[in] callback The user-defined callback function.
-   * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation.
-   * \param[in] group The callback group for this subscription. NULL for no callback group.
-   * \param[in] ignore_local_publications True to ignore local publications.
-   * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
-   * \param[in] allocator Optional custom allocator.
-   * \return Shared pointer to the created subscription.
-   */
-  /* TODO(jacquelinekay):
-     Windows build breaks when static member function passed as default
-     argument to msg_mem_strat, nullptr is a workaround.
-   */
-  template<
-    typename MessageT,
-    typename CallbackT,
-    typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<
-      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>>
-  // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function
-  [[deprecated(
-    "use create_subscription(const std::string &, const rclcpp::QoS &, CallbackT, ...) instead"
-  )]]
-  std::shared_ptr<SubscriptionT>
-  create_subscription(
-    const std::string & topic_name,
-    CallbackT && callback,
-    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
-    bool ignore_local_publications = false,
-    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
-    msg_mem_strat = nullptr,
-    std::shared_ptr<Alloc> allocator = nullptr);
-
-  /// Create and return a Subscription.
-  /**
-   * \param[in] topic_name The topic to subscribe on.
-   * \param[in] qos_history_depth The depth of the subscription's incoming message queue.
-   * \param[in] callback The user-defined callback function.
-   * \param[in] group The callback group for this subscription. NULL for no callback group.
-   * \param[in] ignore_local_publications True to ignore local publications.
-   * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
-   * \param[in] allocator Optional custom allocator.
-   * \return Shared pointer to the created subscription.
-   */
-  /* TODO(jacquelinekay):
-     Windows build breaks when static member function passed as default
-     argument to msg_mem_strat, nullptr is a workaround.
-   */
-  template<
-    typename MessageT,
-    typename CallbackT,
-    typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<
-      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>>
-  // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function
-  [[deprecated(
-    "use create_subscription(const std::string &, const rclcpp::QoS &, CallbackT, ...) instead"
-  )]]
-  std::shared_ptr<SubscriptionT>
-  create_subscription(
-    const std::string & topic_name,
-    CallbackT && callback,
-    size_t qos_history_depth,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
-    bool ignore_local_publications = false,
-    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
-    msg_mem_strat = nullptr,
-    std::shared_ptr<Alloc> allocator = nullptr);
+    typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
+      MessageMemoryStrategyT::create_default()
+    )
+  );
 
   /// Create a timer.
   /**
@@ -369,6 +261,8 @@ public:
    * are ignored, and should be specified using the name argument to this
    * function and the default value's type instead.
    *
+   * If `ignore_override` is `true`, the parameter override will be ignored.
+   *
    * This method, if successful, will result in any callback registered with
    * set_on_parameters_set_callback to be called.
    * If that callback prevents the initial value for the parameter from being
@@ -382,6 +276,8 @@ public:
    *   did not override it.
    * \param[in] parameter_descriptor An optional, custom description for
    *   the parameter.
+   * \param[in] ignore_override When `true`, the parameter override is ignored.
+   *    Default to `false`.
    * \return A const reference to the value of the parameter.
    * \throws rclcpp::exceptions::ParameterAlreadyDeclaredException if parameter
    *   has already been declared.
@@ -396,7 +292,8 @@ public:
     const std::string & name,
     const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
     const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
-    rcl_interfaces::msg::ParameterDescriptor());
+    rcl_interfaces::msg::ParameterDescriptor(),
+    bool ignore_override = false);
 
   /// Declare and initialize a parameter with a type.
   /**
@@ -425,7 +322,8 @@ public:
     const std::string & name,
     const ParameterT & default_value,
     const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
-    rcl_interfaces::msg::ParameterDescriptor());
+    rcl_interfaces::msg::ParameterDescriptor(),
+    bool ignore_override = false);
 
   /// Declare and initialize several parameters with the same namespace and type.
   /**
@@ -440,11 +338,12 @@ public:
    * expanding "namespace.key".
    * This allows you to declare several parameters at once without a namespace.
    *
-   * The map may either contain default values for parameters, or a std::pair
-   * where the first element is a default value and the second is a
-   * parameter descriptor.
-   * This function only takes the default value, but there is another overload
-   * which takes the std::pair with the default value and descriptor.
+   * The map contains default values for parameters.
+   * There is another overload which takes the std::pair with the default value
+   * and descriptor.
+   *
+   * If `ignore_overrides` is `true`, all the overrides of the parameters declared
+   * by the function call will be ignored.
    *
    * This method, if successful, will result in any callback registered with
    * set_on_parameters_set_callback to be called, once for each parameter.
@@ -453,6 +352,8 @@ public:
    *
    * \param[in] namespace_ The namespace in which to declare the parameters.
    * \param[in] parameters The parameters to set in the given namespace.
+   * \param[in] ignore_overrides When `true`, the parameters overrides are ignored.
+   *    Default to `false`.
    * \throws rclcpp::exceptions::ParameterAlreadyDeclaredException if parameter
    *   has already been declared.
    * \throws rclcpp::exceptions::InvalidParametersException if a parameter
@@ -464,7 +365,8 @@ public:
   std::vector<ParameterT>
   declare_parameters(
     const std::string & namespace_,
-    const std::map<std::string, ParameterT> & parameters);
+    const std::map<std::string, ParameterT> & parameters,
+    bool ignore_overrides = false);
 
   /// Declare and initialize several parameters with the same namespace and type.
   /**
@@ -480,7 +382,8 @@ public:
     const std::map<
       std::string,
       std::pair<ParameterT, rcl_interfaces::msg::ParameterDescriptor>
-    > & parameters);
+    > & parameters,
+    bool ignore_overrides = false);
 
   /// Undeclare a previously declared parameter.
   /**
@@ -516,6 +419,7 @@ public:
    * rclcpp::NodeOptions::allow_undeclared_parameters set to true.
    * If undeclared parameters are allowed, then the parameter is implicitly
    * declared with the default parameter meta data before being set.
+   * Parameter overrides are ignored by set_parameter.
    *
    * This method will result in any callback registered with
    * set_on_parameters_set_callback to be called.
@@ -610,38 +514,6 @@ public:
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
 
-  /// Set one parameter, unless that parameter has already been set.
-  /**
-   * Set the given parameter unless already set.
-   *
-   * Deprecated, instead use declare_parameter().
-   *
-   * \param[in] parameters The vector of parameters to be set.
-   * \return The result of each set action as a vector.
-   */
-  template<typename ParameterT>
-  // cppcheck-suppress syntaxError // bug in cppcheck 1.82 for [[deprecated]] on templated function
-  [[deprecated("use declare_parameter() instead")]]
-  void
-  set_parameter_if_not_set(const std::string & name, const ParameterT & value);
-
-  /// Set a map of parameters with the same prefix.
-  /**
-   * For each key in the map, a parameter with a name of "name.key" will be set
-   * to the value in the map.
-   *
-   * Deprecated, instead use declare_parameters().
-   *
-   * \param[in] name The prefix of the parameters to set.
-   * \param[in] values The parameters to set in the given prefix.
-   */
-  template<typename ParameterT>
-  [[deprecated("use declare_parameters() instead")]]
-  void
-  set_parameters_if_not_set(
-    const std::string & name,
-    const std::map<std::string, ParameterT> & values);
-
   /// Return the parameter by the given name.
   /**
    * If the parameter has not been declared, then this method may throw the
@@ -785,28 +657,6 @@ public:
     const std::string & prefix,
     std::map<std::string, ParameterT> & values) const;
 
-  /// Get the parameter value; if not set, set the "alternative value" and store it in the node.
-  /**
-   * If the parameter is set, then the "value" argument is assigned the value
-   * in the parameter.
-   * If the parameter is not set, then the "value" argument is assigned the "alternative_value",
-   * and the parameter is set to the "alternative_value" on the node.
-   *
-   * Deprecated, instead use declare_parameter()'s return value, or use
-   * has_parameter() to ensure it exists before getting it.
-   *
-   * \param[in] name The name of the parameter to get.
-   * \param[out] value The output where the value of the parameter should be assigned.
-   * \param[in] alternative_value Value to be used if the parameter was not set.
-   */
-  template<typename ParameterT>
-  [[deprecated("use declare_parameter() and it's return value instead")]]
-  void
-  get_parameter_or_set(
-    const std::string & name,
-    ParameterT & value,
-    const ParameterT & alternative_value);
-
   /// Return the parameter descriptor for the given parameter name.
   /**
    * Like get_parameters(), this method may throw the
@@ -877,10 +727,12 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
 
+  using OnSetParametersCallbackHandle =
+    rclcpp::node_interfaces::OnSetParametersCallbackHandle;
   using OnParametersSetCallbackType =
     rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
 
-  /// Register a callback to be called anytime a parameter is about to be changed.
+  /// Add a callback for when parameters are being set.
   /**
    * The callback signature is designed to allow handling of any of the above
    * `set_parameter*` or `declare_parameter*` methods, and so it takes a const
@@ -890,19 +742,21 @@ public:
    *
    * For an example callback:
    *
-   *   rcl_interfaces::msg::SetParametersResult
-   *   my_callback(const std::vector<rclcpp::Parameter> & parameters)
-   *   {
-   *     rcl_interfaces::msg::SetParametersResult result;
-   *     result.successful = true;
-   *     for (const auto & parameter : parameters) {
-   *       if (!some_condition) {
-   *         result.successful = false;
-   *         result.reason = "the reason it could not be allowed";
-   *       }
+   * ```cpp
+   * rcl_interfaces::msg::SetParametersResult
+   * my_callback(const std::vector<rclcpp::Parameter> & parameters)
+   * {
+   *   rcl_interfaces::msg::SetParametersResult result;
+   *   result.successful = true;
+   *   for (const auto & parameter : parameters) {
+   *     if (!some_condition) {
+   *       result.successful = false;
+   *       result.reason = "the reason it could not be allowed";
    *     }
-   *     return result;
    *   }
+   *   return result;
+   * }
+   * ```
    *
    * You can see that the SetParametersResult is a boolean flag for success
    * and an optional reason that can be used in error reporting when it fails.
@@ -916,9 +770,68 @@ public:
    *
    * Some constraints like read_only are enforced before the callback is called.
    *
-   * There may only be one callback set at a time, so the previously set
-   * callback is returned when this method is used, or nullptr will be returned
-   * if no callback was previously set.
+   * The callback may introspect other already set parameters (by calling any
+   * of the {get,list,describe}_parameter() methods), but may *not* modify
+   * other parameters (by calling any of the {set,declare}_parameter() methods)
+   * or modify the registered callback itself (by calling the
+   * set_on_parameters_set_callback() method).  If a callback tries to do any
+   * of the latter things,
+   * rclcpp::exceptions::ParameterModifiedInCallbackException will be thrown.
+   *
+   * The callback functions must remain valid as long as the
+   * returned smart pointer is valid.
+   * The returned smart pointer can be promoted to a shared version.
+   *
+   * Resetting or letting the smart pointer go out of scope unregisters the callback.
+   * `remove_on_set_parameters_callback` can also be used.
+   *
+   * The registered callbacks are called when a parameter is set.
+   * When a callback returns a not successful result, the remaining callbacks aren't called.
+   * The order of the callback is the reverse from the registration order.
+   *
+   * \param callback The callback to register.
+   * \returns A shared pointer. The callback is valid as long as the smart pointer is alive.
+   * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
+   */
+  RCLCPP_PUBLIC
+  OnSetParametersCallbackHandle::SharedPtr
+  add_on_set_parameters_callback(OnParametersSetCallbackType callback);
+
+  /// Remove a callback registered with `add_on_set_parameters_callback`.
+  /**
+   * Delete a handler returned by `add_on_set_parameters_callback`.
+   *
+   * e.g.:
+   *
+   *    `remove_on_set_parameters_callback(scoped_callback.get())`
+   *
+   * As an alternative, the smart pointer can be reset:
+   *
+   *    `scoped_callback.reset()`
+   *
+   * Supposing that `scoped_callback` was the only owner.
+   *
+   * Calling `remove_on_set_parameters_callback` more than once with the same handler,
+   * or calling it after the shared pointer has been reset is an error.
+   * Resetting or letting the smart pointer go out of scope after calling
+   * `remove_on_set_parameters_callback` is not a problem.
+   *
+   * \param handler The callback handler to remove.
+   * \throws std::runtime_error if the handler was not created with `add_on_set_parameters_callback`,
+   *   or if it has been removed before.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler);
+
+  /// Register a callback to be called anytime a parameter is about to be changed.
+  /**
+   * With this method, only one callback can be set at a time. The callback that was previously
+   * set by this method is returned or `nullptr` if no callback was previously set.
+   *
+   * The callbacks added with `add_on_set_parameters_callback` are stored in a different place.
+   * `remove_on_set_parameters_callback` can't be used with the callbacks registered with this
+   * method. For removing it, use `set_on_parameters_set_callback(nullptr)`.
    *
    * \param[in] callback The callback to be called when the value for a
    *   parameter is about to be set.
@@ -926,20 +839,9 @@ public:
    *   otherwise nullptr.
    */
   RCLCPP_PUBLIC
-  rclcpp::Node::OnParametersSetCallbackType
+  OnParametersSetCallbackType
   set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback);
 
-  /// Register the callback for parameter changes
-  /**
-   * \param[in] callback User defined callback function.
-   *   It is expected to atomically set parameters.
-   * \note Repeated invocations of this function will overwrite previous callbacks.
-   */
-  template<typename CallbackT>
-  [[deprecated("use set_on_parameters_set_callback() instead")]]
-  void
-  register_param_change_callback(CallbackT && callback);
-
   /// Get the fully-qualified names of all available nodes.
   /**
    * The fully-qualified name includes the local namespace and name of the node.
@@ -1054,15 +956,17 @@ public:
    *
    * For example, consider:
    *
-   *   auto node = std::make_shared<rclcpp::Node>("my_node", "my_ns");
-   *   node->get_sub_namespace();  // -> ""
-   *   auto sub_node1 = node->create_sub_node("a");
-   *   sub_node1->get_sub_namespace();  // -> "a"
-   *   auto sub_node2 = sub_node1->create_sub_node("b");
-   *   sub_node2->get_sub_namespace();  // -> "a/b"
-   *   auto sub_node3 = node->create_sub_node("foo");
-   *   sub_node3->get_sub_namespace();  // -> "foo"
-   *   node->get_sub_namespace();  // -> ""
+   * ```cpp
+   * auto node = std::make_shared<rclcpp::Node>("my_node", "my_ns");
+   * node->get_sub_namespace();  // -> ""
+   * auto sub_node1 = node->create_sub_node("a");
+   * sub_node1->get_sub_namespace();  // -> "a"
+   * auto sub_node2 = sub_node1->create_sub_node("b");
+   * sub_node2->get_sub_namespace();  // -> "a/b"
+   * auto sub_node3 = node->create_sub_node("foo");
+   * sub_node3->get_sub_namespace();  // -> "foo"
+   * node->get_sub_namespace();  // -> ""
+   * ```
    *
    * get_namespace() will return the original node namespace, and will not
    * include the sub-namespace if one exists.
@@ -1084,15 +988,17 @@ public:
    *
    * For example, consider:
    *
-   *   auto node = std::make_shared<rclcpp::Node>("my_node", "my_ns");
-   *   node->get_effective_namespace();  // -> "/my_ns"
-   *   auto sub_node1 = node->create_sub_node("a");
-   *   sub_node1->get_effective_namespace();  // -> "/my_ns/a"
-   *   auto sub_node2 = sub_node1->create_sub_node("b");
-   *   sub_node2->get_effective_namespace();  // -> "/my_ns/a/b"
-   *   auto sub_node3 = node->create_sub_node("foo");
-   *   sub_node3->get_effective_namespace();  // -> "/my_ns/foo"
-   *   node->get_effective_namespace();  // -> "/my_ns"
+   * ```cpp
+   * auto node = std::make_shared<rclcpp::Node>("my_node", "my_ns");
+   * node->get_effective_namespace();  // -> "/my_ns"
+   * auto sub_node1 = node->create_sub_node("a");
+   * sub_node1->get_effective_namespace();  // -> "/my_ns/a"
+   * auto sub_node2 = sub_node1->create_sub_node("b");
+   * sub_node2->get_effective_namespace();  // -> "/my_ns/a/b"
+   * auto sub_node3 = node->create_sub_node("foo");
+   * sub_node3->get_effective_namespace();  // -> "/my_ns/foo"
+   * node->get_effective_namespace();  // -> "/my_ns"
+   * ```
    *
    * \sa get_namespace()
    * \sa get_sub_namespace()

rclcpp/include/rclcpp/node_impl.hpp

@@ -36,10 +36,10 @@
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
 #include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/create_client.hpp"
 #include "rclcpp/create_publisher.hpp"
 #include "rclcpp/create_service.hpp"
 #include "rclcpp/create_subscription.hpp"
-#include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/type_support_decl.hpp"
@@ -78,48 +78,20 @@ Node::create_publisher(
     options);
 }
 
-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<AllocatorT> allocator)
-{
-  PublisherOptionsWithAllocator<AllocatorT> pub_options;
-  pub_options.allocator = allocator;
-  return this->create_publisher<MessageT, AllocatorT, PublisherT>(
-    topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), pub_options);
-}
-
-template<typename MessageT, typename AllocatorT, typename PublisherT>
-std::shared_ptr<PublisherT>
-Node::create_publisher(
-  const std::string & topic_name,
-  const rmw_qos_profile_t & qos_profile,
-  std::shared_ptr<AllocatorT> allocator)
-{
-  rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(qos_profile));
-  qos.get_rmw_qos_profile() = qos_profile;
-
-  PublisherOptionsWithAllocator<AllocatorT> pub_options;
-  pub_options.allocator = allocator;
-  return this->create_publisher<MessageT, AllocatorT, PublisherT>(topic_name, qos, pub_options);
-}
-
 template<
   typename MessageT,
   typename CallbackT,
   typename AllocatorT,
-  typename SubscriptionT>
+  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 rclcpp::message_memory_strategy::MessageMemoryStrategy<
-    typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, AllocatorT>::SharedPtr
-  msg_mem_strat)
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
 {
   return rclcpp::create_subscription<MessageT>(
     *this,
@@ -130,65 +102,6 @@ Node::create_subscription(
     msg_mem_strat);
 }
 
-template<
-  typename MessageT,
-  typename CallbackT,
-  typename Alloc,
-  typename SubscriptionT>
-std::shared_ptr<SubscriptionT>
-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<
-    typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
-  msg_mem_strat,
-  std::shared_ptr<Alloc> allocator)
-{
-  rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(qos_profile));
-  qos.get_rmw_qos_profile() = qos_profile;
-
-  SubscriptionOptionsWithAllocator<Alloc> sub_options;
-  sub_options.callback_group = group;
-  sub_options.ignore_local_publications = ignore_local_publications;
-  sub_options.allocator = allocator;
-
-  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
-    topic_name, qos, std::forward<CallbackT>(callback), sub_options, msg_mem_strat);
-}
-
-template<
-  typename MessageT,
-  typename CallbackT,
-  typename Alloc,
-  typename SubscriptionT>
-std::shared_ptr<SubscriptionT>
-Node::create_subscription(
-  const std::string & topic_name,
-  CallbackT && callback,
-  size_t qos_history_depth,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group,
-  bool ignore_local_publications,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-    typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
-  msg_mem_strat,
-  std::shared_ptr<Alloc> allocator)
-{
-  SubscriptionOptionsWithAllocator<Alloc> sub_options;
-  sub_options.callback_group = group;
-  sub_options.ignore_local_publications = ignore_local_publications;
-  sub_options.allocator = allocator;
-
-  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
-    topic_name,
-    rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)),
-    std::forward<CallbackT>(callback),
-    sub_options,
-    msg_mem_strat);
-}
-
 template<typename DurationRepT, typename DurationT, typename CallbackT>
 typename rclcpp::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(
@@ -211,21 +124,13 @@ Node::create_client(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  rcl_client_options_t options = rcl_client_get_default_options();
-  options.qos = qos_profile;
-
-  using rclcpp::Client;
-  using rclcpp::ClientBase;
-
-  auto cli = Client<ServiceT>::make_shared(
-    node_base_.get(),
+  return rclcpp::create_client<ServiceT>(
+    node_base_,
     node_graph_,
+    node_services_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
-    options);
-
-  auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
-  node_services_->add_client(cli_base_ptr, group);
-  return cli;
+    qos_profile,
+    group);
 }
 
 template<typename ServiceT, typename CallbackT>
@@ -250,12 +155,14 @@ auto
 Node::declare_parameter(
   const std::string & name,
   const ParameterT & default_value,
-  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor)
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
+  bool ignore_override)
 {
   return this->declare_parameter(
     name,
     rclcpp::ParameterValue(default_value),
-    parameter_descriptor
+    parameter_descriptor,
+    ignore_override
   ).get<ParameterT>();
 }
 
@@ -263,14 +170,19 @@ template<typename ParameterT>
 std::vector<ParameterT>
 Node::declare_parameters(
   const std::string & namespace_,
-  const std::map<std::string, ParameterT> & parameters)
+  const std::map<std::string, ParameterT> & 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](auto element) {
-      return this->declare_parameter(normalized_namespace + element.first, element.second);
+    [this, &normalized_namespace, ignore_overrides](auto element) {
+      return this->declare_parameter(
+        normalized_namespace + element.first,
+        element.second,
+        rcl_interfaces::msg::ParameterDescriptor(),
+        ignore_overrides);
     }
   );
   return result;
@@ -283,62 +195,26 @@ Node::declare_parameters(
   const std::map<
     std::string,
     std::pair<ParameterT, rcl_interfaces::msg::ParameterDescriptor>
-  > & parameters)
+  > & 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](auto element) {
+    [this, &normalized_namespace, ignore_overrides](auto element) {
       return static_cast<ParameterT>(
         this->declare_parameter(
           normalized_namespace + element.first,
           element.second.first,
-          element.second.second)
+          element.second.second,
+          ignore_overrides)
       );
     }
   );
   return result;
 }
 
-template<typename ParameterT>
-void
-Node::set_parameter_if_not_set(
-  const std::string & name,
-  const ParameterT & value)
-{
-  if (
-    !this->has_parameter(name) ||
-    this->describe_parameter(name).type == PARAMETER_NOT_SET)
-  {
-    this->set_parameter(rclcpp::Parameter(name, value));
-  }
-}
-
-// this is a partially-specialized version of set_parameter_if_not_set 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>
-void
-Node::set_parameters_if_not_set(
-  const std::string & name,
-  const std::map<std::string, ParameterT> & values)
-{
-  std::vector<rclcpp::Parameter> params;
-
-  for (const auto & val : values) {
-    std::string parameter_name = name + "." + val.first;
-    if (
-      !this->has_parameter(parameter_name) ||
-      this->describe_parameter(parameter_name).type == PARAMETER_NOT_SET)
-    {
-      params.push_back(rclcpp::Parameter(parameter_name, val.second));
-    }
-  }
-
-  this->set_parameters(params);
-}
-
 template<typename ParameterT>
 bool
 Node::get_parameter(const std::string & name, ParameterT & parameter) const
@@ -391,31 +267,6 @@ Node::get_parameters(
   return result;
 }
 
-template<typename ParameterT>
-void
-Node::get_parameter_or_set(
-  const std::string & name,
-  ParameterT & value,
-  const ParameterT & alternative_value)
-{
-  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
-
-  bool got_parameter = get_parameter(sub_name, value);
-  if (!got_parameter) {
-    this->set_parameters({
-        rclcpp::Parameter(sub_name, alternative_value),
-      });
-    value = alternative_value;
-  }
-}
-
-template<typename CallbackT>
-void
-Node::register_param_change_callback(CallbackT && callback)
-{
-  this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
-}
-
 }  // namespace rclcpp
 
 #endif  // RCLCPP__NODE_IMPL_HPP_

rclcpp/include/rclcpp/node_interfaces/get_node_base_interface.hpp

@@ -0,0 +1,149 @@
+// 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_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_
+
+#include <memory>
+#include <utility>
+#include <type_traits>
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+
+/// This header provides the get_node_base_interface() template function.
+/**
+ * This function is useful for getting the NodeBaseInterface pointer from
+ * various kinds of Node-like classes.
+ *
+ * It's able to get the NodeBaseInterface pointer so long as the class
+ * has a method called ``get_node_base_interface()`` which returns
+ * either a pointer (const or not) to a NodeBaseInterface or a
+ * std::shared_ptr to a NodeBaseInterface.
+ */
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+namespace detail
+{
+
+// This is a meta-programming checker for if a given Node-like object has a
+// getter called get_node_base_interface() which returns various types,
+// e.g. const pointer or a shared pointer.
+template<typename NodeType, typename ReturnType>
+struct has_get_node_base_interface
+{
+private:
+  template<typename T>
+  static constexpr
+  auto
+  check(T *)->typename std::is_same<
+    decltype(std::declval<T>().get_node_base_interface()),
+    ReturnType
+  >::type;
+
+  template<typename>
+  static constexpr
+  std::false_type
+  check(...);
+
+public:
+  using type = decltype(check<NodeType>(nullptr));
+  static constexpr bool value = type::value;
+};
+
+// If NodeType is a pointer to NodeBaseInterface already (just normal function overload).
+inline
+rclcpp::node_interfaces::NodeBaseInterface *
+get_node_base_interface_from_pointer(rclcpp::node_interfaces::NodeBaseInterface * pointer)
+{
+  return pointer;
+}
+
+// If NodeType has a method called get_node_base_interface() which returns a shared pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_get_node_base_interface<
+    typename std::remove_pointer<NodeType>::type,
+    std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>
+  >::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeBaseInterface *
+get_node_base_interface_from_pointer(NodeType node_pointer)
+{
+  return node_pointer->get_node_base_interface().get();
+}
+
+// If NodeType has a method called get_node_base_interface() which returns a pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_get_node_base_interface<
+    typename std::remove_pointer<NodeType>::type,
+    rclcpp::node_interfaces::NodeBaseInterface *
+  >::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeBaseInterface *
+get_node_base_interface_from_pointer(NodeType node_pointer)
+{
+  return node_pointer->get_node_base_interface();
+}
+
+// Forward shared_ptr's to const node pointer signatures.
+template<
+  typename NodeType,
+  typename std::enable_if<std::is_same<
+    NodeType,
+    typename std::shared_ptr<typename std::remove_pointer<NodeType>::type::element_type> *
+  >::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeBaseInterface *
+get_node_base_interface_from_pointer(NodeType node_shared_pointer)
+{
+  return get_node_base_interface_from_pointer(node_shared_pointer->get());
+}
+
+}  // namespace detail
+
+/// Get the NodeBaseInterface as a pointer from a pointer to a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeBaseInterface *
+get_node_base_interface(NodeType node_pointer)
+{
+  // Forward pointers to detail implmentation directly.
+  return detail::get_node_base_interface_from_pointer(node_pointer);
+}
+
+/// Get the NodeBaseInterface as a pointer from a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<
+    !std::is_pointer<typename std::remove_reference<NodeType>::type>::value, int
+  >::type = 0
+>
+rclcpp::node_interfaces::NodeBaseInterface *
+get_node_base_interface(NodeType && node_reference)
+{
+  // Forward references to detail implmentation as a pointer.
+  return detail::get_node_base_interface_from_pointer(&node_reference);
+}
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_BASE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/get_node_timers_interface.hpp

@@ -0,0 +1,149 @@
+// 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_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_
+
+#include <memory>
+#include <utility>
+#include <type_traits>
+
+#include "rclcpp/node_interfaces/node_timers_interface.hpp"
+
+/// This header provides the get_node_timers_interface() template function.
+/**
+ * This function is useful for getting the NodeTimersInterface pointer from
+ * various kinds of Node-like classes.
+ *
+ * It's able to get the NodeTimersInterface pointer so long as the class
+ * has a method called ``get_node_timers_interface()`` which returns
+ * either a pointer (const or not) to a NodeTimersInterface or a
+ * std::shared_ptr to a NodeTimersInterface.
+ */
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+namespace detail
+{
+
+// This is a meta-programming checker for if a given Node-like object has a
+// getter called get_node_timers_interface() which returns various types,
+// e.g. const pointer or a shared pointer.
+template<typename NodeType, typename ReturnType>
+struct has_get_node_timers_interface
+{
+private:
+  template<typename T>
+  static constexpr
+  auto
+  check(T *)->typename std::is_same<
+    decltype(std::declval<T>().get_node_timers_interface()),
+    ReturnType
+  >::type;
+
+  template<typename>
+  static constexpr
+  std::false_type
+  check(...);
+
+public:
+  using type = decltype(check<NodeType>(nullptr));
+  static constexpr bool value = type::value;
+};
+
+// If NodeType is a pointer to NodeTimersInterface already (just normal function overload).
+inline
+rclcpp::node_interfaces::NodeTimersInterface *
+get_node_timers_interface_from_pointer(rclcpp::node_interfaces::NodeTimersInterface * pointer)
+{
+  return pointer;
+}
+
+// If NodeType has a method called get_node_timers_interface() which returns a shared pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_get_node_timers_interface<
+    typename std::remove_pointer<NodeType>::type,
+    std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface>
+  >::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeTimersInterface *
+get_node_timers_interface_from_pointer(NodeType node_pointer)
+{
+  return node_pointer->get_node_timers_interface().get();
+}
+
+// If NodeType has a method called get_node_timers_interface() which returns a pointer.
+template<
+  typename NodeType,
+  typename std::enable_if<has_get_node_timers_interface<
+    typename std::remove_pointer<NodeType>::type,
+    rclcpp::node_interfaces::NodeTimersInterface *
+  >::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeTimersInterface *
+get_node_timers_interface_from_pointer(NodeType node_pointer)
+{
+  return node_pointer->get_node_timers_interface();
+}
+
+// Forward shared_ptr's to const node pointer signatures.
+template<
+  typename NodeType,
+  typename std::enable_if<std::is_same<
+    NodeType,
+    typename std::shared_ptr<typename std::remove_pointer<NodeType>::type::element_type> *
+  >::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeTimersInterface *
+get_node_timers_interface_from_pointer(NodeType node_shared_pointer)
+{
+  return get_node_timers_interface_from_pointer(node_shared_pointer->get());
+}
+
+}  // namespace detail
+
+/// Get the NodeTimersInterface as a pointer from a pointer to a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
+>
+rclcpp::node_interfaces::NodeTimersInterface *
+get_node_timers_interface(NodeType node_pointer)
+{
+  // Forward pointers to detail implmentation directly.
+  return detail::get_node_timers_interface_from_pointer(node_pointer);
+}
+
+/// Get the NodeTimersInterface as a pointer from a "Node like" object.
+template<
+  typename NodeType,
+  typename std::enable_if<
+    !std::is_pointer<typename std::remove_reference<NodeType>::type>::value, int
+  >::type = 0
+>
+rclcpp::node_interfaces::NodeTimersInterface *
+get_node_timers_interface(NodeType && node_reference)
+{
+  // Forward references to detail implmentation as a pointer.
+  return detail::get_node_timers_interface_from_pointer(&node_reference);
+}
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_TIMERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/get_node_topics_interface.hpp

@@ -123,16 +123,18 @@ template<
   typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
 >
 rclcpp::node_interfaces::NodeTopicsInterface *
-get_node_topics_interface(NodeType && node_pointer)
+get_node_topics_interface(NodeType node_pointer)
 {
   // Forward pointers to detail implmentation directly.
-  return detail::get_node_topics_interface_from_pointer(std::forward<NodeType>(node_pointer));
+  return detail::get_node_topics_interface_from_pointer(node_pointer);
 }
 
 /// Get the NodeTopicsInterface as a pointer from a "Node like" object.
 template<
   typename NodeType,
-  typename std::enable_if<!std::is_pointer<NodeType>::value, int>::type = 0
+  typename std::enable_if<
+    !std::is_pointer<typename std::remove_reference<NodeType>::type>::value, int
+  >::type = 0
 >
 rclcpp::node_interfaces::NodeTopicsInterface *
 get_node_topics_interface(NodeType && node_reference)

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -17,6 +17,7 @@
 
 #include <map>
 #include <memory>
+#include <list>
 #include <string>
 #include <vector>
 
@@ -50,6 +51,30 @@ struct ParameterInfo
   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
 {
@@ -80,7 +105,8 @@ public:
   declare_parameter(
     const std::string & name,
     const rclcpp::ParameterValue & default_value,
-    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor) override;
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
+    bool ignore_override) override;
 
   RCLCPP_PUBLIC
   void
@@ -133,25 +159,37 @@ public:
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
   RCLCPP_PUBLIC
-  OnParametersSetCallbackType
-  set_on_parameters_set_callback(OnParametersSetCallbackType callback) override;
+  OnSetParametersCallbackHandle::SharedPtr
+  add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
 
-  [[deprecated("use set_on_parameters_set_callback() instead")]]
   RCLCPP_PUBLIC
   void
-  register_param_change_callback(OnParametersSetCallbackType callback) override;
+  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::mutex mutex_;
+  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_;

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 
 #include <map>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -32,6 +33,18 @@ 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
 {
@@ -51,8 +64,10 @@ public:
   const rclcpp::ParameterValue &
   declare_parameter(
     const std::string & name,
-    const rclcpp::ParameterValue & default_value,
-    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor) = 0;
+    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.
   /**
@@ -156,13 +171,25 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
 
-  using OnParametersSetCallbackType =
-    std::function<
-    rcl_interfaces::msg::SetParametersResult(const std::vector<rclcpp::Parameter> &)
-    >;
+  using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType;
 
-  using ParametersCallbackFunction [[deprecated("use OnParametersSetCallbackType instead")]] =
-    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.
   /**
@@ -173,12 +200,6 @@ public:
   OnParametersSetCallbackType
   set_on_parameters_set_callback(OnParametersSetCallbackType callback) = 0;
 
-  [[deprecated("use set_on_parameters_set_callback() instead")]]
-  RCLCPP_PUBLIC
-  virtual
-  void
-  register_param_change_callback(OnParametersSetCallbackType callback) = 0;
-
   /// Return the initial parameter values used by the NodeParameters to override default values.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_topics.hpp

@@ -49,8 +49,7 @@ public:
   create_publisher(
     const std::string & topic_name,
     const rclcpp::PublisherFactory & publisher_factory,
-    const rcl_publisher_options_t & publisher_options,
-    bool use_intra_process) override;
+    const rclcpp::QoS & qos) override;
 
   RCLCPP_PUBLIC
   void
@@ -63,8 +62,7 @@ public:
   create_subscription(
     const std::string & topic_name,
     const rclcpp::SubscriptionFactory & subscription_factory,
-    const rcl_subscription_options_t & subscription_options,
-    bool use_intra_process) override;
+    const rclcpp::QoS & qos) override;
 
   RCLCPP_PUBLIC
   void

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -50,8 +50,7 @@ public:
   create_publisher(
     const std::string & topic_name,
     const rclcpp::PublisherFactory & publisher_factory,
-    const rcl_publisher_options_t & publisher_options,
-    bool use_intra_process) = 0;
+    const rclcpp::QoS & qos) = 0;
 
   RCLCPP_PUBLIC
   virtual
@@ -66,8 +65,7 @@ public:
   create_subscription(
     const std::string & topic_name,
     const rclcpp::SubscriptionFactory & subscription_factory,
-    const rcl_subscription_options_t & subscription_options,
-    bool use_intra_process) = 0;
+    const rclcpp::QoS & qos) = 0;
 
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_options.hpp

@@ -99,7 +99,7 @@ public:
   /// Set the arguments, return this for parameter idiom.
   /**
    * These arguments are used to extract remappings used by the node and other
-   * settings.
+   * 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/include/rclcpp/parameter_client.hpp

@@ -53,19 +53,22 @@ public:
     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);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
   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);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
+    rclcpp::callback_group::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);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
   RCLCPP_PUBLIC
   std::shared_future<std::vector<rclcpp::Parameter>>
@@ -115,7 +118,7 @@ public:
   on_parameter_event(
     CallbackT && callback,
     const rclcpp::QoS & qos = (
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default))
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))
     ),
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
       rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
@@ -142,7 +145,7 @@ public:
     NodeT && node,
     CallbackT && callback,
     const rclcpp::QoS & qos = (
-      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default))
+      rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))
     ),
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options = (
       rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>()
@@ -268,7 +271,11 @@ public:
   {
     return get_parameter_impl(
       parameter_name,
-      std::function<T()>([]() -> T {throw std::runtime_error("Parameter not set");}));
+      std::function<T()>([&parameter_name]() -> T
+      {
+        throw std::runtime_error("Parameter '" + parameter_name + "' is not set");
+      })
+    );
   }
 
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/parameter_events_filter.hpp

@@ -44,16 +44,20 @@ public:
    * \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.
-  *    auto res = rclcpp::ParameterEventsFilter(
-  *      event_shared_ptr,
-  *      {"foo", "bar"},
-  *      {rclcpp::ParameterEventsFilter::EventType::NEW, rclcpp::ParameterEventsFilter::EventType::CHANGED});
-  */
+   *    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,

rclcpp/include/rclcpp/publisher.hpp

@@ -28,67 +28,126 @@
 #include "rcl/error_handling.h"
 #include "rcl/publisher.h"
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
-
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
-#include "rclcpp/intra_process_manager.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
 {
 
+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
 {
 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(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const std::string & topic,
-    const rcl_publisher_options_t & publisher_options,
-    const PublisherEventCallbacks & event_callbacks,
-    const std::shared_ptr<MessageAlloc> & allocator)
+    const rclcpp::QoS & qos,
+    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
   : PublisherBase(
       node_base,
       topic,
       *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
-      publisher_options),
-    message_allocator_(allocator)
+      options.template to_rcl_publisher_options<MessageT>(qos)),
+    options_(options),
+    message_allocator_(new MessageAllocator(*options.get_allocator().get()))
   {
     allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
 
-    if (event_callbacks.deadline_callback) {
-      this->add_event_handler(event_callbacks.deadline_callback,
+    if (options_.event_callbacks.deadline_callback) {
+      this->add_event_handler(
+        options_.event_callbacks.deadline_callback,
         RCL_PUBLISHER_OFFERED_DEADLINE_MISSED);
     }
-    if (event_callbacks.liveliness_callback) {
-      this->add_event_handler(event_callbacks.liveliness_callback,
+    if (options_.event_callbacks.liveliness_callback) {
+      this->add_event_handler(
+        options_.event_callbacks.liveliness_callback,
         RCL_PUBLISHER_LIVELINESS_LOST);
     }
+
+    // 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()
   {}
 
-  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  make_mapped_ring_buffer(size_t size) const override
+  /// 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 mapped_ring_buffer::MappedRingBuffer<
-      MessageT,
-      typename Publisher<MessageT, Alloc>::MessageAlloc
-    >::make_shared(size, this->get_allocator());
+    return rclcpp::LoanedMessage<MessageT, AllocatorT>(this, this->get_allocator());
   }
 
   /// Send a message to the topic for this publisher.
@@ -100,7 +159,7 @@ public:
   publish(std::unique_ptr<MessageT, MessageDeleter> msg)
   {
     if (!intra_process_is_enabled_) {
-      this->do_inter_process_publish(msg.get());
+      this->do_inter_process_publish(*msg);
       return;
     }
     // If an interprocess subscription exist, then the unique_ptr is promoted
@@ -109,34 +168,15 @@ public:
     // 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.
-    uint64_t message_seq;
     bool inter_process_publish_needed =
       get_subscription_count() > get_intra_process_subscription_count();
-    MessageSharedPtr shared_msg;
-    if (inter_process_publish_needed) {
-      shared_msg = std::move(msg);
-      message_seq =
-        store_intra_process_message(intra_process_publisher_id_, shared_msg);
-    } else {
-      message_seq =
-        store_intra_process_message(intra_process_publisher_id_, std::move(msg));
-    }
-    this->do_intra_process_publish(message_seq);
-    if (inter_process_publish_needed) {
-      this->do_inter_process_publish(shared_msg.get());
-    }
-  }
 
-// Skip deprecated attribute in windows, as it raise a warning in template specialization.
-#if !defined(_WIN32)
-  [[deprecated(
-    "publishing an unique_ptr is prefered when using intra process communication."
-    " If using a shared_ptr, use publish(*msg).")]]
-#endif
-  virtual void
-  publish(const std::shared_ptr<const MessageT> & msg)
-  {
-    publish(*msg);
+    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 {
+      this->do_intra_process_publish(std::move(msg));
+    }
   }
 
   virtual void
@@ -145,69 +185,70 @@ public:
     // Avoid allocating when not using intra process.
     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.
     // As the message is not const, a copy should be made.
     // A shared_ptr<const MessageT> could also be constructed here.
-    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
-    MessageAllocTraits::construct(*message_allocator_.get(), ptr, msg);
+    auto ptr = MessageAllocatorTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocatorTraits::construct(*message_allocator_.get(), ptr, msg);
     MessageUniquePtr unique_msg(ptr, message_deleter_);
     this->publish(std::move(unique_msg));
   }
 
-// Skip deprecated attribute in windows, as it raise a warning in template specialization.
-#if !defined(_WIN32)
-  [[deprecated(
-    "Use publish(*msg). Check against nullptr before calling if necessary.")]]
-#endif
-  virtual void
-  publish(const MessageT * msg)
-  {
-    if (!msg) {
-      throw std::runtime_error("msg argument is nullptr");
-    }
-    return this->publish(*msg);
-  }
-
   void
   publish(const rcl_serialized_message_t & serialized_msg)
   {
     return this->do_serialized_publish(&serialized_msg);
   }
 
-// Skip deprecated attribute in windows, as it raise a warning in template specialization.
-#if !defined(_WIN32)
-  [[deprecated(
-    "Use publish(*serialized_msg). Check against nullptr before calling if necessary.")]]
-#endif
+  /// 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(const rcl_serialized_message_t * serialized_msg)
+  publish(rclcpp::LoanedMessage<MessageT, AllocatorT> && loaned_msg)
   {
-    return this->do_serialized_publish(serialized_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());
+    }
   }
 
-// Skip deprecated attribute in windows, as it raise a warning in template specialization.
-#if !defined(_WIN32)
-  [[deprecated(
-    "Use publish(*serialized_msg). Check against nullptr before calling if necessary.")]]
-#endif
-  void
-  publish(std::shared_ptr<const rcl_serialized_message_t> serialized_msg)
-  {
-    return this->do_serialized_publish(serialized_msg.get());
-  }
-
-  std::shared_ptr<MessageAlloc> get_allocator() const
+  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 = rcl_publish(&publisher_handle_, msg, nullptr);
+    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_)) {
@@ -237,16 +278,14 @@ protected:
   }
 
   void
-  do_intra_process_publish(uint64_t message_seq)
+  do_loaned_message_publish(MessageT * msg)
   {
-    rcl_interfaces::msg::IntraProcessMessage ipm;
-    ipm.publisher_id = intra_process_publisher_id_;
-    ipm.message_sequence = message_seq;
-    auto status = rcl_publish(&intra_process_publisher_handle_, &ipm, nullptr);
+    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(&intra_process_publisher_handle_)) {
-        rcl_context_t * context = rcl_publisher_get_context(&intra_process_publisher_handle_);
+      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;
@@ -254,14 +293,12 @@ protected:
       }
     }
     if (RCL_RET_OK != status) {
-      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish intra process message");
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message");
     }
   }
 
-  uint64_t
-  store_intra_process_message(
-    uint64_t publisher_id,
-    std::shared_ptr<const MessageT> msg)
+  void
+  do_intra_process_publish(std::unique_ptr<MessageT, MessageDeleter> msg)
   {
     auto ipm = weak_ipm_.lock();
     if (!ipm) {
@@ -269,17 +306,17 @@ protected:
               "intra process publish called after destruction of intra process manager");
     }
     if (!msg) {
-      throw std::runtime_error("cannot publisher msg which is a null pointer");
+      throw std::runtime_error("cannot publish msg which is a null pointer");
     }
-    uint64_t message_seq =
-      ipm->template store_intra_process_message<MessageT, Alloc>(publisher_id, msg);
-    return message_seq;
+
+    ipm->template do_intra_process_publish<MessageT, AllocatorT>(
+      intra_process_publisher_id_,
+      std::move(msg),
+      message_allocator_);
   }
 
-  uint64_t
-  store_intra_process_message(
-    uint64_t publisher_id,
-    std::unique_ptr<MessageT, MessageDeleter> msg)
+  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) {
@@ -287,14 +324,23 @@ protected:
               "intra process publish called after destruction of intra process manager");
     }
     if (!msg) {
-      throw std::runtime_error("cannot publisher msg which is a null pointer");
+      throw std::runtime_error("cannot publish msg which is a null pointer");
     }
-    uint64_t message_seq =
-      ipm->template store_intra_process_message<MessageT, Alloc>(publisher_id, std::move(msg));
-    return message_seq;
+
+    return ipm->template do_intra_process_publish_and_return_shared<MessageT, AllocatorT>(
+      intra_process_publisher_id_,
+      std::move(msg),
+      message_allocator_);
   }
 
-  std::shared_ptr<MessageAlloc> 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_;
 };

rclcpp/include/rclcpp/publisher_base.hpp

@@ -28,7 +28,7 @@
 #include "rcl/publisher.h"
 
 #include "rclcpp/macros.hpp"
-#include "rclcpp/mapped_ring_buffer.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -41,18 +41,18 @@ namespace node_interfaces
 {
 class NodeBaseInterface;
 class NodeTopicsInterface;
-}
+}  // namespace node_interfaces
 
-namespace intra_process_manager
+namespace experimental
 {
 /**
  * IntraProcessManager is forward declared here, avoiding a circular inclusion between
  * `intra_process_manager.hpp` and `publisher_base.hpp`.
  */
 class IntraProcessManager;
-}
+}  // namespace experimental
 
-class PublisherBase
+class PublisherBase : public std::enable_shared_from_this<PublisherBase>
 {
   friend ::rclcpp::node_interfaces::NodeTopicsInterface;
 
@@ -96,12 +96,6 @@ public:
   const rmw_gid_t &
   get_gid() const;
 
-  /// Get the global identifier for this publisher used by intra-process communication.
-  /** \return The intra-process gid. */
-  RCLCPP_PUBLIC
-  const rmw_gid_t &
-  get_intra_process_gid() const;
-
   /// Get the rcl publisher handle.
   /** \return The rcl publisher handle. */
   RCLCPP_PUBLIC
@@ -153,12 +147,22 @@ public:
    * If the underlying setting in use can't be represented in ROS terms,
    * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
    * May throw runtime_error when an unexpected error occurs.
+   *
    * \return The actual qos settings.
    */
   RCLCPP_PUBLIC
-  rmw_qos_profile_t
+  rclcpp::QoS
   get_actual_qos() const;
 
+  /// Check if publisher instance can loan messages.
+  /**
+   * Depending on the middleware and the message type, this will return true if the middleware
+   * can allocate a ROS message instance.
+   */
+  RCLCPP_PUBLIC
+  bool
+  can_loan_messages() const;
+
   /// Compare this publisher to a gid.
   /**
    * Note that this function calls the next function.
@@ -180,20 +184,14 @@ public:
   operator==(const rmw_gid_t * gid) const;
 
   using IntraProcessManagerSharedPtr =
-    std::shared_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
-
-  /// Implementation utility function that creates a typed mapped ring buffer.
-  RCLCPP_PUBLIC
-  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-  virtual make_mapped_ring_buffer(size_t size) const;
+    std::shared_ptr<rclcpp::experimental::IntraProcessManager>;
 
   /// Implementation utility function used to setup intra process publishing after creation.
   RCLCPP_PUBLIC
   void
   setup_intra_process(
     uint64_t intra_process_publisher_id,
-    IntraProcessManagerSharedPtr ipm,
-    const rcl_publisher_options_t & intra_process_options);
+    IntraProcessManagerSharedPtr ipm);
 
 protected:
   template<typename EventCallbackT>
@@ -213,18 +211,16 @@ protected:
   std::shared_ptr<rcl_node_t> rcl_node_handle_;
 
   rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
-  rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher();
 
   std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> event_handlers_;
 
   using IntraProcessManagerWeakPtr =
-    std::weak_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
+    std::weak_ptr<rclcpp::experimental::IntraProcessManager>;
   bool intra_process_is_enabled_;
   IntraProcessManagerWeakPtr weak_ipm_;
   uint64_t intra_process_publisher_id_;
 
   rmw_gid_t rmw_gid_;
-  rmw_gid_t intra_process_rmw_gid_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/publisher_factory.hpp

@@ -24,8 +24,10 @@
 #include "rosidl_typesupport_cpp/message_type_support.hpp"
 
 #include "rclcpp/publisher.hpp"
-#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/publisher_base.hpp"
+#include "rclcpp/publisher_options.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -41,6 +43,9 @@ namespace rclcpp
  * called from a templated "create_publisher" method on the Node class, and
  * is passed to the non-templated "create_publisher" method on the NodeTopics
  * class where it is used to create and setup the Publisher.
+ *
+ * It also handles the two step construction of Publishers, first calling
+ * the constructor and then the post_init_setup() method.
  */
 struct PublisherFactory
 {
@@ -49,39 +54,33 @@ struct PublisherFactory
     rclcpp::PublisherBase::SharedPtr(
       rclcpp::node_interfaces::NodeBaseInterface * node_base,
       const std::string & topic_name,
-      const rcl_publisher_options_t & publisher_options)>;
+      const rclcpp::QoS & qos
+    )>;
 
-  PublisherFactoryFunction create_typed_publisher;
+  const PublisherFactoryFunction create_typed_publisher;
 };
 
-/// Return a PublisherFactory with functions setup for creating a PublisherT<MessageT, Alloc>.
-template<typename MessageT, typename Alloc, typename PublisherT>
+/// Return a PublisherFactory with functions setup for creating a PublisherT<MessageT, AllocatorT>.
+template<typename MessageT, typename AllocatorT, typename PublisherT>
 PublisherFactory
-create_publisher_factory(
-  const PublisherEventCallbacks & event_callbacks,
-  std::shared_ptr<Alloc> allocator)
+create_publisher_factory(const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
 {
-  PublisherFactory factory;
-
-  // factory function that creates a MessageT specific PublisherT
-  factory.create_typed_publisher =
-    [event_callbacks, allocator](
-    rclcpp::node_interfaces::NodeBaseInterface * node_base,
-    const std::string & topic_name,
-    const rcl_publisher_options_t & publisher_options
+  PublisherFactory factory {
+    // factory function that creates a MessageT specific PublisherT
+    [options](
+      rclcpp::node_interfaces::NodeBaseInterface * node_base,
+      const std::string & topic_name,
+      const rclcpp::QoS & qos
     ) -> std::shared_ptr<PublisherT>
     {
-      auto options_copy = publisher_options;
-      auto message_alloc = std::make_shared<typename PublisherT::MessageAlloc>(*allocator.get());
-      options_copy.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc.get());
-
-      return std::make_shared<PublisherT>(
-        node_base,
-        topic_name,
-        options_copy,
-        event_callbacks,
-        message_alloc);
-    };
+      auto publisher = std::make_shared<PublisherT>(node_base, topic_name, qos, options);
+      // This is used for setting up things like intra process comms which
+      // require this->shared_from_this() which cannot be called from
+      // the constructor.
+      publisher->post_init_setup(node_base, topic_name, qos, options);
+      return publisher;
+    }
+  };
 
   // return the factory now that it is populated
   return factory;

rclcpp/include/rclcpp/publisher_options.hpp

@@ -19,16 +19,22 @@
 #include <string>
 #include <vector>
 
-#include "rclcpp/callback_group.hpp"
+#include "rcl/publisher.h"
+
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp"
 #include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
-#include "rclcpp/visibility_control.hpp"
-#include "rcl/publisher.h"
 
 namespace rclcpp
 {
 
+namespace callback_group
+{
+class CallbackGroup;
+}  // namespace callback_group
+
 /// Non-templated part of PublisherOptionsWithAllocator<Allocator>.
 struct PublisherOptionsBase
 {
@@ -39,7 +45,11 @@ struct PublisherOptionsBase
   PublisherEventCallbacks event_callbacks;
 
   /// Callback group in which the waitable items from the publisher should be placed.
-  rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
+  std::shared_ptr<rclcpp::callback_group::CallbackGroup> callback_group;
+
+  /// Optional RMW implementation specific payload to be used during creation of the publisher.
+  std::shared_ptr<rclcpp::detail::RMWImplementationSpecificPublisherPayload>
+  rmw_implementation_payload = nullptr;
 };
 
 /// Structure containing optional configuration for Publishers.
@@ -64,11 +74,33 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
     rcl_publisher_options_t result;
     using AllocatorTraits = std::allocator_traits<Allocator>;
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-    auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
-    result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
+    result.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
+
+    // Apply payload to rcl_publisher_options if necessary.
+    if (rmw_implementation_payload && rmw_implementation_payload->has_been_customized()) {
+      rmw_implementation_payload->modify_rmw_publisher_options(result.rmw_publisher_options);
+    }
+
     return result;
   }
+
+
+  /// Get the allocator, creating one if needed.
+  std::shared_ptr<Allocator>
+  get_allocator() const
+  {
+    if (!this->allocator) {
+      // TODO(wjwwood): I would like to use the commented line instead, but
+      //   cppcheck 1.89 fails with:
+      //     Syntax Error: AST broken, binary operator '>' doesn't have two operands.
+      // return std::make_shared<Allocator>();
+      std::shared_ptr<Allocator> tmp(new Allocator());
+      return tmp;
+    }
+    return this->allocator;
+  }
 };
 
 using PublisherOptions = PublisherOptionsWithAllocator<std::allocator<void>>;

rclcpp/include/rclcpp/rclcpp.hpp

@@ -48,7 +48,8 @@
  *   - rclcpp::Node::get_parameter()
  *   - rclcpp::Node::describe_parameters()
  *   - rclcpp::Node::list_parameters()
- *   - rclcpp::Node::register_param_change_callback()
+ *   - rclcpp::Node::add_on_set_parameters_callback()
+ *   - rclcpp::Node::remove_on_set_parameters_callback()
  *   - rclcpp::Parameter
  *   - rclcpp::ParameterValue
  *   - rclcpp::AsyncParametersClient

rclcpp/include/rclcpp/service.hpp

@@ -33,6 +33,7 @@
 #include "rclcpp/logging.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
+#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -154,6 +155,10 @@ public:
 
       rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
     }
+    TRACEPOINT(
+      rclcpp_service_callback_added,
+      (const void *)get_service_handle().get(),
+      (const void *)&any_callback_);
   }
 
   Service(
@@ -172,6 +177,10 @@ public:
     }
 
     service_handle_ = service_handle;
+    TRACEPOINT(
+      rclcpp_service_callback_added,
+      (const void *)get_service_handle().get(),
+      (const void *)&any_callback_);
   }
 
   Service(
@@ -192,6 +201,10 @@ public:
     // In this case, rcl owns the service handle memory
     service_handle_ = std::shared_ptr<rcl_service_t>(new rcl_service_t);
     service_handle_->impl = service_handle->impl;
+    TRACEPOINT(
+      rclcpp_service_callback_added,
+      (const void *)get_service_handle().get(),
+      (const void *)&any_callback_);
   }
 
   Service() = delete;

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -164,40 +164,28 @@ public:
         if (!group || !group->can_be_taken_from().load()) {
           continue;
         }
-        for (auto & weak_subscription : group->get_subscription_ptrs()) {
-          auto subscription = weak_subscription.lock();
-          if (subscription) {
+        group->find_subscription_ptrs_if(
+          [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
             subscription_handles_.push_back(subscription->get_subscription_handle());
-            if (subscription->get_intra_process_subscription_handle()) {
-              subscription_handles_.push_back(
-                subscription->get_intra_process_subscription_handle());
-            }
-          }
-        }
-        for (auto & weak_service : group->get_service_ptrs()) {
-          auto service = weak_service.lock();
-          if (service) {
+            return false;
+          });
+
+        group->find_service_ptrs_if([this](const rclcpp::ServiceBase::SharedPtr & service) {
             service_handles_.push_back(service->get_service_handle());
-          }
-        }
-        for (auto & weak_client : group->get_client_ptrs()) {
-          auto client = weak_client.lock();
-          if (client) {
+            return false;
+          });
+        group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) {
             client_handles_.push_back(client->get_client_handle());
-          }
-        }
-        for (auto & weak_timer : group->get_timer_ptrs()) {
-          auto timer = weak_timer.lock();
-          if (timer) {
+            return false;
+          });
+        group->find_timer_ptrs_if([this](const rclcpp::TimerBase::SharedPtr & timer) {
             timer_handles_.push_back(timer->get_timer_handle());
-          }
-        }
-        for (auto & weak_waitable : group->get_waitable_ptrs()) {
-          auto waitable = weak_waitable.lock();
-          if (waitable) {
+            return false;
+          });
+        group->find_waitable_ptrs_if([this](const rclcpp::Waitable::SharedPtr & waitable) {
             waitable_handles_.push_back(waitable);
-          }
-        }
+            return false;
+          });
       }
     }
     return has_invalid_weak_nodes;
@@ -271,11 +259,6 @@ public:
     while (it != subscription_handles_.end()) {
       auto subscription = get_subscription_by_handle(*it, weak_nodes);
       if (subscription) {
-        // Figure out if this is for intra-process or not.
-        bool is_intra_process = false;
-        if (subscription->get_intra_process_subscription_handle()) {
-          is_intra_process = subscription->get_intra_process_subscription_handle() == *it;
-        }
         // Find the group for this handle and see if it can be serviced
         auto group = get_group_by_subscription(subscription, weak_nodes);
         if (!group) {
@@ -291,11 +274,7 @@ public:
           continue;
         }
         // Otherwise it is safe to set and return the any_exec
-        if (is_intra_process) {
-          any_exec.subscription_intra_process = subscription;
-        } else {
-          any_exec.subscription = subscription;
-        }
+        any_exec.subscription = subscription;
         any_exec.callback_group = group;
         any_exec.node_base = get_node_by_group(group, weak_nodes);
         subscription_handles_.erase(it);
@@ -374,6 +353,41 @@ public:
     }
   }
 
+  virtual void
+  get_next_timer(
+    executor::AnyExecutable & any_exec,
+    const WeakNodeList & weak_nodes)
+  {
+    auto it = timer_handles_.begin();
+    while (it != timer_handles_.end()) {
+      auto timer = get_timer_by_handle(*it, weak_nodes);
+      if (timer) {
+        // Find the group for this handle and see if it can be serviced
+        auto group = get_group_by_timer(timer, weak_nodes);
+        if (!group) {
+          // Group was not found, meaning the timer is not valid...
+          // Remove it from the ready list and continue looking
+          it = timer_handles_.erase(it);
+          continue;
+        }
+        if (!group->can_be_taken_from().load()) {
+          // Group is mutually exclusive and is being used, so skip it for now
+          // Leave it to be checked next time, but continue searching
+          ++it;
+          continue;
+        }
+        // Otherwise it is safe to set and return the any_exec
+        any_exec.timer = timer;
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        timer_handles_.erase(it);
+        return;
+      }
+      // Else, the service is no longer valid, remove it and continue
+      it = timer_handles_.erase(it);
+    }
+  }
+
   virtual void
   get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes)
   {

rclcpp/include/rclcpp/subscription.hpp

@@ -29,20 +29,24 @@
 #include "rcl/error_handling.h"
 #include "rcl/subscription.h"
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
-
 #include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/detail/resolve_use_intra_process.hpp"
+#include "rclcpp/detail/resolve_intra_process_buffer_type.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
-#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/experimental/intra_process_manager.hpp"
+#include "rclcpp/experimental/subscription_intra_process.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/subscription_base.hpp"
+#include "rclcpp/subscription_options.hpp"
 #include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/waitable.hpp"
+#include "tracetools/tracetools.h"
 
 namespace rclcpp
 {
@@ -55,15 +59,19 @@ class NodeTopicsInterface;
 /// Subscription implementation, templated on the type of message this subscription receives.
 template<
   typename CallbackMessageT,
-  typename Alloc = std::allocator<void>>
+  typename AllocatorT = std::allocator<void>,
+  typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT,
+    AllocatorT
+  >>
 class Subscription : public SubscriptionBase
 {
   friend class rclcpp::node_interfaces::NodeTopicsInterface;
 
 public:
-  using MessageAllocTraits = allocator::AllocRebind<CallbackMessageT, Alloc>;
-  using MessageAlloc = typename MessageAllocTraits::allocator_type;
-  using MessageDeleter = allocator::Deleter<MessageAlloc, CallbackMessageT>;
+  using MessageAllocatorTraits = allocator::AllocRebind<CallbackMessageT, AllocatorT>;
+  using MessageAllocator = typename MessageAllocatorTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAllocator, CallbackMessageT>;
   using ConstMessageSharedPtr = std::shared_ptr<const CallbackMessageT>;
   using MessageUniquePtr = std::unique_ptr<CallbackMessageT, MessageDeleter>;
 
@@ -71,42 +79,115 @@ public:
 
   /// Default constructor.
   /**
-   * The constructor for a subscription is almost never called directly. Instead, subscriptions
-   * should be instantiated through Node::create_subscription.
-   * \param[in] node_handle rcl representation of the node that owns this subscription.
+   * The constructor for a subscription is almost never called directly.
+   * Instead, subscriptions should be instantiated through the function
+   * rclcpp::create_subscription().
+   *
+   * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
    * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
    * \param[in] topic_name Name of the topic to subscribe to.
-   * \param[in] subscription_options options for the subscription.
    * \param[in] callback User defined callback to call when a message is received.
-   * \param[in] memory_strategy The memory strategy to be used for managing message memory.
+   * \param[in] options options for the subscription.
+   * \param[in] message_memory_strategy The memory strategy to be used for managing message memory.
    */
   Subscription(
-    std::shared_ptr<rcl_node_t> node_handle,
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
-    const rcl_subscription_options_t & subscription_options,
-    AnySubscriptionCallback<CallbackMessageT, Alloc> callback,
-    const SubscriptionEventCallbacks & event_callbacks,
-    typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, Alloc>::SharedPtr
-    memory_strategy = message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
-    Alloc>::create_default())
+    const rclcpp::QoS & qos,
+    AnySubscriptionCallback<CallbackMessageT, AllocatorT> callback,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
+    typename MessageMemoryStrategyT::SharedPtr message_memory_strategy)
   : SubscriptionBase(
-      node_handle,
+      node_base,
       type_support_handle,
       topic_name,
-      subscription_options,
+      options.template to_rcl_subscription_options<CallbackMessageT>(qos),
       rclcpp::subscription_traits::is_serialized_subscription_argument<CallbackMessageT>::value),
     any_callback_(callback),
-    message_memory_strategy_(memory_strategy)
+    options_(options),
+    message_memory_strategy_(message_memory_strategy)
   {
-    if (event_callbacks.deadline_callback) {
-      this->add_event_handler(event_callbacks.deadline_callback,
+    if (options.event_callbacks.deadline_callback) {
+      this->add_event_handler(
+        options.event_callbacks.deadline_callback,
         RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
     }
-    if (event_callbacks.liveliness_callback) {
-      this->add_event_handler(event_callbacks.liveliness_callback,
+    if (options.event_callbacks.liveliness_callback) {
+      this->add_event_handler(
+        options.event_callbacks.liveliness_callback,
         RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
     }
+
+    // Setup intra process publishing if requested.
+    if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) {
+      using rclcpp::detail::resolve_intra_process_buffer_type;
+
+      // Check if the QoS is compatible with intra-process.
+      rmw_qos_profile_t qos_profile = get_actual_qos().get_rmw_qos_profile();
+      if (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_profile.depth == 0) {
+        throw std::invalid_argument(
+                "intraprocess communication is not allowed with 0 depth qos policy");
+      }
+      if (qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
+        throw std::invalid_argument(
+                "intraprocess communication allowed only with volatile durability");
+      }
+
+      // First create a SubscriptionIntraProcess which will be given to the intra-process manager.
+      auto context = node_base->get_context();
+      auto subscription_intra_process = std::make_shared<
+        rclcpp::experimental::SubscriptionIntraProcess<
+          CallbackMessageT,
+          AllocatorT,
+          typename MessageUniquePtr::deleter_type
+        >>(
+        callback,
+        options.get_allocator(),
+        context,
+        this->get_topic_name(),    // important to get like this, as it has the fully-qualified name
+        qos_profile,
+        resolve_intra_process_buffer_type(options.intra_process_buffer_type, callback)
+        );
+      TRACEPOINT(
+        rclcpp_subscription_init,
+        (const void *)get_subscription_handle().get(),
+        (const void *)subscription_intra_process.get());
+
+      // Add it to the intra process manager.
+      using rclcpp::experimental::IntraProcessManager;
+      auto ipm = context->get_sub_context<IntraProcessManager>();
+      uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process);
+      this->setup_intra_process(intra_process_subscription_id, ipm);
+    }
+
+    TRACEPOINT(
+      rclcpp_subscription_init,
+      (const void *)get_subscription_handle().get(),
+      (const void *)this);
+    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.
+    any_callback_.register_callback_for_tracing();
+  }
+
+  /// Called after construction to continue setup that requires shared_from_this().
+  void post_init_setup(
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
+    const rclcpp::QoS & qos,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
+  {
+    (void)node_base;
+    (void)qos;
+    (void)options;
   }
 
   /// Support dynamically setting the message memory strategy.
@@ -116,12 +197,12 @@ public:
    */
   void set_message_memory_strategy(
     typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
-    Alloc>::SharedPtr message_memory_strategy)
+    AllocatorT>::SharedPtr message_memory_strategy)
   {
     message_memory_strategy_ = message_memory_strategy;
   }
 
-  std::shared_ptr<void> create_message()
+  std::shared_ptr<void> create_message() override
   {
     /* The default message memory strategy provides a dynamically allocated message on each call to
      * create_message, though alternative memory strategies that re-use a preallocated message may be
@@ -130,12 +211,13 @@ public:
     return message_memory_strategy_->borrow_message();
   }
 
-  std::shared_ptr<rcl_serialized_message_t> create_serialized_message()
+  std::shared_ptr<rcl_serialized_message_t> create_serialized_message() override
   {
     return message_memory_strategy_->borrow_serialized_message();
   }
 
-  void handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info)
+  void handle_message(
+    std::shared_ptr<void> & message, const rmw_message_info_t & message_info) override
   {
     if (matches_any_intra_process_publishers(&message_info.publisher_gid)) {
       // In this case, the message will be delivered via intra process and
@@ -146,127 +228,46 @@ public:
     any_callback_.dispatch(typed_message, message_info);
   }
 
-  /// Return the loaned message.
+  void
+  handle_loaned_message(
+    void * loaned_message, const rmw_message_info_t & message_info) override
+  {
+    auto typed_message = static_cast<CallbackMessageT *>(loaned_message);
+    // message is loaned, so we have to make sure that the deleter does not deallocate the message
+    auto sptr = std::shared_ptr<CallbackMessageT>(
+      typed_message, [](CallbackMessageT * msg) {(void) msg;});
+    any_callback_.dispatch(sptr, message_info);
+  }
+
+  /// Return the borrowed message.
   /** \param message message to be returned */
-  void return_message(std::shared_ptr<void> & message)
+  void return_message(std::shared_ptr<void> & message) override
   {
     auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     message_memory_strategy_->return_message(typed_message);
   }
 
-  void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message)
+  void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message) override
   {
     message_memory_strategy_->return_serialized_message(message);
   }
 
-  void handle_intra_process_message(
-    rcl_interfaces::msg::IntraProcessMessage & ipm,
-    const rmw_message_info_t & message_info)
+  bool use_take_shared_method() const
   {
-    if (!use_intra_process_) {
-      // throw std::runtime_error(
-      //   "handle_intra_process_message called before setup_intra_process");
-      // TODO(wjwwood): for now, this could mean that intra process was just not enabled.
-      // However, this can only really happen if this node has it disabled, but the other doesn't.
-      return;
-    }
-    if (any_callback_.use_take_shared_method()) {
-      ConstMessageSharedPtr msg;
-      take_intra_process_message(
-        ipm.publisher_id,
-        ipm.message_sequence,
-        intra_process_subscription_id_,
-        msg);
-      if (!msg) {
-        // This can happen when having two nodes in different process both using intraprocess
-        // communication. It could happen too if the publisher no longer exists or the requested
-        // message is not longer being stored.
-        // TODO(ivanpauno): Print a warn message in the last two cases described above,
-        // but not in the first one.
-        return;
-      }
-      any_callback_.dispatch_intra_process(msg, message_info);
-    } else {
-      MessageUniquePtr msg;
-      take_intra_process_message(
-        ipm.publisher_id,
-        ipm.message_sequence,
-        intra_process_subscription_id_,
-        msg);
-      if (!msg) {
-        // This can happen when having two nodes in different process both using intraprocess
-        // communication. It could happen too if the publisher no longer exists or the requested
-        // message is not longer being stored.
-        // TODO(ivanpauno): Print a warn message in the last two cases described above,
-        // but not in the first one.
-        return;
-      }
-      any_callback_.dispatch_intra_process(std::move(msg), message_info);
-    }
-  }
-
-  /// Implemenation detail.
-  const std::shared_ptr<rcl_subscription_t>
-  get_intra_process_subscription_handle() const
-  {
-    if (!use_intra_process_) {
-      return nullptr;
-    }
-    return intra_process_subscription_handle_;
+    return any_callback_.use_take_shared_method();
   }
 
 private:
-  void
-  take_intra_process_message(
-    uint64_t publisher_id,
-    uint64_t message_sequence,
-    uint64_t subscription_id,
-    MessageUniquePtr & message)
-  {
-    auto ipm = weak_ipm_.lock();
-    if (!ipm) {
-      throw std::runtime_error(
-              "intra process take called after destruction of intra process manager");
-    }
-    ipm->template take_intra_process_message<CallbackMessageT, Alloc>(
-      publisher_id, message_sequence, subscription_id, message);
-  }
-
-  void
-  take_intra_process_message(
-    uint64_t publisher_id,
-    uint64_t message_sequence,
-    uint64_t subscription_id,
-    ConstMessageSharedPtr & message)
-  {
-    auto ipm = weak_ipm_.lock();
-    if (!ipm) {
-      throw std::runtime_error(
-              "intra process take called after destruction of intra process manager");
-    }
-    ipm->template take_intra_process_message<CallbackMessageT, Alloc>(
-      publisher_id, message_sequence, subscription_id, message);
-  }
-
-  bool
-  matches_any_intra_process_publishers(const rmw_gid_t * sender_gid)
-  {
-    if (!use_intra_process_) {
-      return false;
-    }
-    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);
-  }
-
   RCLCPP_DISABLE_COPY(Subscription)
 
-  AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
-  typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, Alloc>::SharedPtr
+  AnySubscriptionCallback<CallbackMessageT, AllocatorT> any_callback_;
+  /// 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 subscription.
+   */
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> options_;
+  typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, AllocatorT>::SharedPtr
     message_memory_strategy_;
 };
 

rclcpp/include/rclcpp/subscription_base.hpp

@@ -21,12 +21,13 @@
 
 #include "rcl/subscription.h"
 
-#include "rcl_interfaces/msg/intra_process_message.hpp"
-
 #include "rmw/rmw.h"
 
 #include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/experimental/intra_process_manager.hpp"
+#include "rclcpp/experimental/subscription_intra_process_base.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -36,28 +37,28 @@ namespace rclcpp
 
 namespace node_interfaces
 {
-class NodeTopicsInterface;
+class NodeBaseInterface;
 }  // namespace node_interfaces
 
-namespace intra_process_manager
+namespace experimental
 {
 /**
  * IntraProcessManager is forward declared here, avoiding a circular inclusion between
  * `intra_process_manager.hpp` and `subscription_base.hpp`.
  */
 class IntraProcessManager;
-}
+}  // namespace experimental
 
 /// Virtual base class for subscriptions. This pattern allows us to iterate over different template
 /// specializations of Subscription, among other things.
-class SubscriptionBase
+class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase)
 
   /// Default constructor.
   /**
-   * \param[in] node_handle The rcl representation of the node that owns this subscription.
+   * \param[in] node_base NodeBaseInterface pointer used in parts of the setup.
    * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
    * \param[in] topic_name Name of the topic to subscribe to.
    * \param[in] subscription_options options for the subscription.
@@ -65,7 +66,7 @@ public:
    */
   RCLCPP_PUBLIC
   SubscriptionBase(
-    std::shared_ptr<rcl_node_t> node_handle,
+    rclcpp::node_interfaces::NodeBaseInterface * node_base,
     const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
     const rcl_subscription_options_t & subscription_options,
@@ -88,24 +89,39 @@ public:
   const std::shared_ptr<rcl_subscription_t>
   get_subscription_handle() const;
 
-  RCLCPP_PUBLIC
-  virtual const std::shared_ptr<rcl_subscription_t>
-  get_intra_process_subscription_handle() const;
-
   /// Get all the QoS event handlers associated with this subscription.
   /** \return The vector of QoS event handlers. */
   RCLCPP_PUBLIC
   const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
   get_event_handlers() const;
 
+  /// Get the actual QoS settings, after the defaults have been determined.
+  /**
+   * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT
+   * can only be resolved after the creation of the publisher, and it
+   * depends on the underlying rmw implementation.
+   * If the underlying setting in use can't be represented in ROS terms,
+   * it will be set to RMW_QOS_POLICY_*_UNKNOWN.
+   * May throw runtime_error when an unexpected error occurs.
+   *
+   * \return The actual qos settings.
+   */
+  RCLCPP_PUBLIC
+  rclcpp::QoS
+  get_actual_qos() const;
+
   /// Borrow a new message.
   /** \return Shared pointer to the fresh message. */
-  virtual std::shared_ptr<void>
+  RCLCPP_PUBLIC
+  virtual
+  std::shared_ptr<void>
   create_message() = 0;
 
   /// Borrow a new serialized message
   /** \return Shared pointer to a rcl_message_serialized_t. */
-  virtual std::shared_ptr<rcl_serialized_message_t>
+  RCLCPP_PUBLIC
+  virtual
+  std::shared_ptr<rcl_serialized_message_t>
   create_serialized_message() = 0;
 
   /// Check if we need to handle the message, and execute the callback if we do.
@@ -113,27 +129,35 @@ public:
    * \param[in] message Shared pointer to the message to handle.
    * \param[in] message_info Metadata associated with this message.
    */
-  virtual void
+  RCLCPP_PUBLIC
+  virtual
+  void
   handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
 
+  RCLCPP_PUBLIC
+  virtual
+  void
+  handle_loaned_message(void * loaned_message, const rmw_message_info_t & message_info) = 0;
+
   /// Return the message borrowed in create_message.
   /** \param[in] message Shared pointer to the returned message. */
-  virtual void
+  RCLCPP_PUBLIC
+  virtual
+  void
   return_message(std::shared_ptr<void> & message) = 0;
 
   /// Return the message borrowed in create_serialized_message.
   /** \param[in] message Shared pointer to the returned message. */
-  virtual void
+  RCLCPP_PUBLIC
+  virtual
+  void
   return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message) = 0;
 
-  virtual void
-  handle_intra_process_message(
-    rcl_interfaces::msg::IntraProcessMessage & ipm,
-    const rmw_message_info_t & message_info) = 0;
-
+  RCLCPP_PUBLIC
   const rosidl_message_type_support_t &
   get_message_type_support_handle() const;
 
+  RCLCPP_PUBLIC
   bool
   is_serialized() const;
 
@@ -143,14 +167,31 @@ public:
   size_t
   get_publisher_count() const;
 
+  /// Check if subscription instance can loan messages.
+  /**
+   * Depending on the middleware and the message type, this will return true if the middleware
+   * can allocate a ROS message instance.
+   *
+   * \return boolean flag indicating if middleware can loan messages.
+   */
+  RCLCPP_PUBLIC
+  bool
+  can_loan_messages() const;
+
   using IntraProcessManagerWeakPtr =
-    std::weak_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
+    std::weak_ptr<rclcpp::experimental::IntraProcessManager>;
 
   /// Implemenation detail.
-  void setup_intra_process(
+  RCLCPP_PUBLIC
+  void
+  setup_intra_process(
     uint64_t intra_process_subscription_id,
-    IntraProcessManagerWeakPtr weak_ipm,
-    const rcl_subscription_options_t & intra_process_options);
+    IntraProcessManagerWeakPtr weak_ipm);
+
+  /// Return the waitable for intra-process, or nullptr if intra-process is not setup.
+  RCLCPP_PUBLIC
+  rclcpp::Waitable::SharedPtr
+  get_intra_process_waitable() const;
 
 protected:
   template<typename EventCallbackT>
@@ -167,6 +208,12 @@ protected:
     event_handlers_.emplace_back(handler);
   }
 
+  RCLCPP_PUBLIC
+  bool
+  matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const;
+
+  rclcpp::node_interfaces::NodeBaseInterface * const node_base_;
+
   std::shared_ptr<rcl_node_t> node_handle_;
   std::shared_ptr<rcl_subscription_t> subscription_handle_;
   std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -24,25 +24,31 @@
 
 #include "rosidl_typesupport_cpp/message_type_support.hpp"
 
-#include "rclcpp/subscription.hpp"
-#include "rclcpp/subscription_traits.hpp"
-#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/intra_process_buffer_type.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/subscription_options.hpp"
+#include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
 {
 
-/// Factory with functions used to create a Subscription<MessageT>.
+/// Factory containing a function used to create a Subscription<MessageT>.
 /**
- * This factory class is used to encapsulate the template generated functions
- * which are used during the creation of a Message type specific subscription
+ * This factory class is used to encapsulate the template generated function
+ * which is used during the creation of a Message type specific subscription
  * within a non-templated class.
  *
  * It is created using the create_subscription_factory function, which is
  * usually called from a templated "create_subscription" method of the Node
  * class, and is passed to the non-templated "create_subscription" method of
  * the NodeTopics class where it is used to create and setup the Subscription.
+ *
+ * It also handles the two step construction of Subscriptions, first calling
+ * the constructor and then the post_init_setup() method.
  */
 struct SubscriptionFactory
 {
@@ -51,71 +57,62 @@ struct SubscriptionFactory
     rclcpp::SubscriptionBase::SharedPtr(
       rclcpp::node_interfaces::NodeBaseInterface * node_base,
       const std::string & topic_name,
-      const rcl_subscription_options_t & subscription_options)>;
+      const rclcpp::QoS & qos)>;
 
-  SubscriptionFactoryFunction create_typed_subscription;
-
-  // Function that takes a MessageT from the intra process manager
-  using SetupIntraProcessFunction = std::function<
-    void (
-      rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
-      rclcpp::SubscriptionBase::SharedPtr subscription,
-      const rcl_subscription_options_t & subscription_options)>;
-
-  SetupIntraProcessFunction setup_intra_process;
+  const SubscriptionFactoryFunction create_typed_subscription;
 };
 
-/// Return a SubscriptionFactory with functions for creating a SubscriptionT<MessageT, Alloc>.
+/// Return a SubscriptionFactory setup to create a SubscriptionT<MessageT, AllocatorT>.
 template<
   typename MessageT,
   typename CallbackT,
-  typename Alloc,
-  typename CallbackMessageT,
-  typename SubscriptionT>
+  typename AllocatorT,
+  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
+  >>
 SubscriptionFactory
 create_subscription_factory(
   CallbackT && callback,
-  const SubscriptionEventCallbacks & event_callbacks,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
-    CallbackMessageT, Alloc>::SharedPtr
-  msg_mem_strat,
-  std::shared_ptr<Alloc> allocator)
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
 {
-  SubscriptionFactory factory;
+  auto allocator = options.get_allocator();
 
   using rclcpp::AnySubscriptionCallback;
-  AnySubscriptionCallback<CallbackMessageT, Alloc> any_subscription_callback(allocator);
+  AnySubscriptionCallback<CallbackMessageT, AllocatorT> any_subscription_callback(allocator);
   any_subscription_callback.set(std::forward<CallbackT>(callback));
 
-  auto message_alloc =
-    std::make_shared<typename Subscription<CallbackMessageT, Alloc>::MessageAlloc>();
-
-  // factory function that creates a MessageT specific SubscriptionT
-  factory.create_typed_subscription =
-    [allocator, msg_mem_strat, any_subscription_callback, event_callbacks, message_alloc](
-    rclcpp::node_interfaces::NodeBaseInterface * node_base,
-    const std::string & topic_name,
-    const rcl_subscription_options_t & subscription_options
+  SubscriptionFactory factory {
+    // factory function that creates a MessageT specific SubscriptionT
+    [options, msg_mem_strat, any_subscription_callback](
+      rclcpp::node_interfaces::NodeBaseInterface * node_base,
+      const std::string & topic_name,
+      const rclcpp::QoS & qos
     ) -> rclcpp::SubscriptionBase::SharedPtr
     {
-      auto options_copy = subscription_options;
-      options_copy.allocator =
-        rclcpp::allocator::get_rcl_allocator<CallbackMessageT>(*message_alloc.get());
-
       using rclcpp::Subscription;
       using rclcpp::SubscriptionBase;
 
-      auto sub = Subscription<CallbackMessageT, Alloc>::make_shared(
-        node_base->get_shared_rcl_node_handle(),
+      auto sub = Subscription<CallbackMessageT, AllocatorT>::make_shared(
+        node_base,
         *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
         topic_name,
-        options_copy,
+        qos,
         any_subscription_callback,
-        event_callbacks,
+        options,
         msg_mem_strat);
+      // This is used for setting up things like intra process comms which
+      // require this->shared_from_this() which cannot be called from
+      // the constructor.
+      sub->post_init_setup(node_base, qos, options);
       auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
       return sub_base_ptr;
-    };
+    }
+  };
 
   // return the factory now that it is populated
   return factory;

rclcpp/include/rclcpp/subscription_options.hpp

@@ -20,6 +20,8 @@
 #include <vector>
 
 #include "rclcpp/callback_group.hpp"
+#include "rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp"
+#include "rclcpp/intra_process_buffer_type.hpp"
 #include "rclcpp/intra_process_setting.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/qos_event.hpp"
@@ -33,12 +35,22 @@ struct SubscriptionOptionsBase
 {
   /// Callbacks for events related to this subscription.
   SubscriptionEventCallbacks event_callbacks;
+
   /// True to ignore local publications.
   bool ignore_local_publications = false;
+
   /// The callback group for this subscription. NULL to use the default callback group.
   rclcpp::callback_group::CallbackGroup::SharedPtr callback_group = nullptr;
+
   /// Setting to explicitly set intraprocess communications.
   IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault;
+
+  /// Setting the data-type stored in the intraprocess buffer
+  IntraProcessBufferType intra_process_buffer_type = IntraProcessBufferType::CallbackDefault;
+
+  /// Optional RMW implementation specific payload to be used during creation of the subscription.
+  std::shared_ptr<rclcpp::detail::RMWImplementationSpecificSubscriptionPayload>
+  rmw_implementation_payload = nullptr;
 };
 
 /// Structure containing optional configuration for Subscriptions.
@@ -61,15 +73,31 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   rcl_subscription_options_t
   to_rcl_subscription_options(const rclcpp::QoS & qos) const
   {
-    rcl_subscription_options_t result;
+    rcl_subscription_options_t result = rcl_subscription_get_default_options();
     using AllocatorTraits = std::allocator_traits<Allocator>;
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
     auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
     result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
-    result.ignore_local_publications = this->ignore_local_publications;
     result.qos = qos.get_rmw_qos_profile();
+    result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;
+
+    // Apply payload to rcl_subscription_options if necessary.
+    if (rmw_implementation_payload && rmw_implementation_payload->has_been_customized()) {
+      rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options);
+    }
+
     return result;
   }
+
+  /// Get the allocator, creating one if needed.
+  std::shared_ptr<Allocator>
+  get_allocator() const
+  {
+    if (!this->allocator) {
+      return std::make_shared<Allocator>();
+    }
+    return this->allocator;
+  }
 };
 
 using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;

rclcpp/include/rclcpp/time_source.hpp

@@ -25,6 +25,7 @@
 #include "rcl_interfaces/msg/parameter_event.hpp"
 
 #include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 
 
 namespace rclcpp
@@ -133,6 +134,8 @@ private:
   std::mutex clock_list_lock_;
   // A vector to store references to associated clocks.
   std::vector<rclcpp::Clock::SharedPtr> associated_clocks_;
+  // A handler for the use_sim_time parameter callback.
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_ = nullptr;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/timer.hpp

@@ -30,6 +30,8 @@
 #include "rclcpp/rate.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "tracetools/tracetools.h"
+#include "tracetools/utils.hpp"
 
 #include "rcl/error_handling.h"
 #include "rcl/timer.h"
@@ -133,6 +135,14 @@ public:
   )
   : TimerBase(clock, period, context), callback_(std::forward<FunctorT>(callback))
   {
+    TRACEPOINT(
+      rclcpp_timer_callback_added,
+      (const void *)get_timer_handle().get(),
+      (const void *)&callback_);
+    TRACEPOINT(
+      rclcpp_callback_register,
+      (const void *)&callback_,
+      get_symbol(callback_));
   }
 
   /// Default destructor.
@@ -152,7 +162,9 @@ public:
     if (ret != RCL_RET_OK) {
       throw std::runtime_error("Failed to notify timer that callback occurred");
     }
+    TRACEPOINT(callback_start, (const void *)&callback_, false);
     execute_callback_delegate<>();
+    TRACEPOINT(callback_end, (const void *)&callback_);
   }
 
   // void specialization

rclcpp/include/rclcpp/waitable.hpp

@@ -128,7 +128,7 @@ public:
    *
    * Example usage:
    *
-   * ```
+   * ```cpp
    * // ... create a wait set and a Waitable
    * // Add the Waitable to the wait set
    * bool add_ret = waitable.add_to_wait_set(wait_set);

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>0.7.5</version>
+  <version>0.8.4</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>
@@ -26,7 +26,9 @@
 
   <depend>rcl</depend>
   <depend>rcl_yaml_param_parser</depend>
+  <depend>rcpputils</depend>
   <depend>rmw_implementation</depend>
+  <depend>tracetools</depend>
 
   <exec_depend>ament_cmake</exec_depend>
 

rclcpp/resource/logging.hpp.em

@@ -45,16 +45,38 @@
 #endif
 
 @{
+from collections import OrderedDict
+from copy import deepcopy
 from rcutils.logging import feature_combinations
-from rcutils.logging import get_macro_parameters
 from rcutils.logging import get_suffix_from_features
 from rcutils.logging import severities
+from rcutils.logging import throttle_args
+from rcutils.logging import throttle_params
 
-# TODO(dhood): Implement the throttle macro using time sources available in rclcpp
-excluded_features = ['named', 'throttle']
-def is_supported_feature_combination(feature_combination):
-    is_excluded = any([ef in feature_combination for ef in excluded_features])
-    return not is_excluded
+throttle_args['condition_before'] = 'RCUTILS_LOG_CONDITION_THROTTLE_BEFORE(clock, duration)'
+del throttle_params['get_time_point_value']
+throttle_params['clock'] = 'rclcpp::Clock that will be used to get the time point.'
+throttle_params.move_to_end('clock', last=False)
+
+rclcpp_feature_combinations = OrderedDict()
+for combinations, feature in feature_combinations.items():
+    # skip feature combinations using 'named'
+    if 'named' in combinations:
+        continue
+    rclcpp_feature_combinations[combinations] = feature
+# add a stream variant for each available feature combination
+stream_arg = 'stream_arg'
+for combinations, feature in list(rclcpp_feature_combinations.items()):
+    combinations = ('stream', ) + combinations
+    feature = deepcopy(feature)
+    feature.params[stream_arg] = 'The argument << into a stringstream'
+    rclcpp_feature_combinations[combinations] = feature
+
+def get_rclcpp_suffix_from_features(features):
+    suffix = get_suffix_from_features(features)
+    if 'stream' in features:
+        suffix = '_STREAM' + suffix
+    return suffix
 }@
 @[for severity in severities]@
 /** @@name Logging macros for severity @(severity).
@@ -62,50 +84,77 @@ def is_supported_feature_combination(feature_combination):
 ///@@{
 #if (RCLCPP_LOG_MIN_SEVERITY > RCLCPP_LOG_MIN_SEVERITY_@(severity))
 // empty logging macros for severity @(severity) when being disabled at compile time
-@[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@
-@{suffix = get_suffix_from_features(feature_combination)}@
+@[ for feature_combination in rclcpp_feature_combinations.keys()]@
+@{suffix = get_rclcpp_suffix_from_features(feature_combination)}@
 /// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY.
 #define RCLCPP_@(severity)@(suffix)(...)
 @[ end for]@
 
 #else
-@[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@
-@{suffix = get_suffix_from_features(feature_combination)}@
+@[ for feature_combination in rclcpp_feature_combinations.keys()]@
+@{suffix = get_rclcpp_suffix_from_features(feature_combination)}@
 // The RCLCPP_@(severity)@(suffix) macro is surrounded by do { .. } while (0)
 // to implement the standard C macro idiom to make the macro safe in all
 // contexts; see http://c-faq.com/cpp/multistmt.html for more information.
 /**
  * \def RCLCPP_@(severity)@(suffix)
  * Log a message with severity @(severity)@
-@[ if feature_combinations[feature_combination].doc_lines]@
+@[ if rclcpp_feature_combinations[feature_combination].doc_lines]@
  with the following conditions:
 @[ else]@
 .
 @[ end if]@
-@[ for doc_line in feature_combinations[feature_combination].doc_lines]@
+@[ for doc_line in rclcpp_feature_combinations[feature_combination].doc_lines]@
  * @(doc_line)
 @[ end for]@
  * \param logger The `rclcpp::Logger` to use
-@[ for param_name, doc_line in feature_combinations[feature_combination].params.items()]@
+@[ for param_name, doc_line in rclcpp_feature_combinations[feature_combination].params.items()]@
  * \param @(param_name) @(doc_line)
 @[ end for]@
+@[ if 'stream' not in feature_combination]@
  * \param ... The format string, followed by the variable arguments for the format string.
  * It also accepts a single argument of type std::string.
+@[ end if]@
  */
-#define RCLCPP_@(severity)@(suffix)(logger, @(''.join([p + ', ' for p in get_macro_parameters(feature_combination).keys()]))...) \
+@{params = rclcpp_feature_combinations[feature_combination].params.keys()}@
+#define RCLCPP_@(severity)@(suffix)(logger@(''.join([', ' + p for p in params]))@
+@[ if 'stream' not in feature_combination]@
+, ...@
+@[ end if]@
+) \
   do { \
     static_assert( \
-      ::std::is_same<typename std::remove_reference<typename std::remove_cv<decltype(logger)>::type>::type, \
+      ::std::is_same<typename std::remove_cv<typename std::remove_reference<decltype(logger)>::type>::type, \
       typename ::rclcpp::Logger>::value, \
       "First argument to logging macros must be an rclcpp::Logger"); \
-    RCUTILS_LOG_@(severity)@(suffix)_NAMED( \
-@{params = get_macro_parameters(feature_combination).keys()}@
+@[ if 'throttle' in feature_combination]@ \
+    auto get_time_point = [&c=clock](rcutils_time_point_value_t * time_point) -> rcutils_ret_t { \
+      try { \
+        *time_point = c.now().nanoseconds(); \
+      } catch (...) { \
+        RCUTILS_SAFE_FWRITE_TO_STDERR( \
+        "[rclcpp|logging.hpp] RCLCPP_@(severity)@(suffix) could not get current time stamp\n"); \
+        return RCUTILS_RET_ERROR; \
+      } \
+        return RCUTILS_RET_OK; \
+    }; \
+@[ end if] \
+@[ if 'stream' in feature_combination]@
+    std::stringstream ss; \
+    ss << @(stream_arg); \
+@[ end if]@
+    RCUTILS_LOG_@(severity)@(get_suffix_from_features(feature_combination))_NAMED( \
+@{params = ['get_time_point' if p == 'clock' and 'throttle' in feature_combination else p for p in params]}@
 @[ if params]@
-@(''.join(['      ' + p + ', \\\n' for p in params]))@
+@(''.join(['      ' + p + ', \\\n' for p in params if p != stream_arg]))@
 @[ end if]@
       logger.get_name(), \
+@[ if 'stream' not in feature_combination]@
       rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \
         RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,"")); \
+@[ else]@
+      "%s", rclcpp::get_c_string(ss.str())); \
+@[ end if]@
   } while (0)
 
 @[ end for]@

rclcpp/src/rclcpp/any_executable.cpp

@@ -18,7 +18,6 @@ using rclcpp::executor::AnyExecutable;
 
 AnyExecutable::AnyExecutable()
 : subscription(nullptr),
-  subscription_intra_process(nullptr),
   timer(nullptr),
   service(nullptr),
   client(nullptr),

rclcpp/src/rclcpp/callback_group.cpp

@@ -23,40 +23,6 @@ CallbackGroup::CallbackGroup(CallbackGroupType group_type)
 : type_(group_type), can_be_taken_from_(true)
 {}
 
-const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
-CallbackGroup::get_subscription_ptrs() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return subscription_ptrs_;
-}
-
-const std::vector<rclcpp::TimerBase::WeakPtr> &
-CallbackGroup::get_timer_ptrs() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return timer_ptrs_;
-}
-
-const std::vector<rclcpp::ServiceBase::WeakPtr> &
-CallbackGroup::get_service_ptrs() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return service_ptrs_;
-}
-
-const std::vector<rclcpp::ClientBase::WeakPtr> &
-CallbackGroup::get_client_ptrs() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return client_ptrs_;
-}
-
-const std::vector<rclcpp::Waitable::WeakPtr> &
-CallbackGroup::get_waitable_ptrs() const
-{
-  std::lock_guard<std::mutex> lock(mutex_);
-  return waitable_ptrs_;
-}
 
 std::atomic_bool &
 CallbackGroup::can_be_taken_from()
@@ -76,6 +42,12 @@ CallbackGroup::add_subscription(
 {
   std::lock_guard<std::mutex> lock(mutex_);
   subscription_ptrs_.push_back(subscription_ptr);
+  subscription_ptrs_.erase(
+    std::remove_if(
+      subscription_ptrs_.begin(),
+      subscription_ptrs_.end(),
+      [](rclcpp::SubscriptionBase::WeakPtr x) {return x.expired();}),
+    subscription_ptrs_.end());
 }
 
 void
@@ -83,6 +55,12 @@ CallbackGroup::add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   timer_ptrs_.push_back(timer_ptr);
+  timer_ptrs_.erase(
+    std::remove_if(
+      timer_ptrs_.begin(),
+      timer_ptrs_.end(),
+      [](rclcpp::TimerBase::WeakPtr x) {return x.expired();}),
+    timer_ptrs_.end());
 }
 
 void
@@ -90,6 +68,12 @@ CallbackGroup::add_service(const rclcpp::ServiceBase::SharedPtr service_ptr)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   service_ptrs_.push_back(service_ptr);
+  service_ptrs_.erase(
+    std::remove_if(
+      service_ptrs_.begin(),
+      service_ptrs_.end(),
+      [](rclcpp::ServiceBase::WeakPtr x) {return x.expired();}),
+    service_ptrs_.end());
 }
 
 void
@@ -97,6 +81,12 @@ CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   client_ptrs_.push_back(client_ptr);
+  client_ptrs_.erase(
+    std::remove_if(
+      client_ptrs_.begin(),
+      client_ptrs_.end(),
+      [](rclcpp::ClientBase::WeakPtr x) {return x.expired();}),
+    client_ptrs_.end());
 }
 
 void
@@ -104,6 +94,12 @@ CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr)
 {
   std::lock_guard<std::mutex> lock(mutex_);
   waitable_ptrs_.push_back(waitable_ptr);
+  waitable_ptrs_.erase(
+    std::remove_if(
+      waitable_ptrs_.begin(),
+      waitable_ptrs_.end(),
+      [](rclcpp::Waitable::WeakPtr x) {return x.expired();}),
+    waitable_ptrs_.end());
 }
 
 void

rclcpp/src/rclcpp/clock.cpp

@@ -119,8 +119,9 @@ Clock::create_jump_callback(
   }
 
   // Try to add the jump callback to the clock
-  rcl_ret_t ret = rcl_clock_add_jump_callback(&rcl_clock_, threshold,
-      Clock::on_time_jump, handler.get());
+  rcl_ret_t ret = rcl_clock_add_jump_callback(
+    &rcl_clock_, threshold, Clock::on_time_jump,
+    handler.get());
   if (RCL_RET_OK != ret) {
     exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
   }

rclcpp/src/rclcpp/detail/rmw_implementation_specific_payload.cpp

@@ -1,4 +1,4 @@
-// Copyright 2015 Open Source Robotics Foundation, Inc.
+// 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.
@@ -12,12 +12,24 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/intra_process_manager_impl.hpp"
+#include <rclcpp/detail/rmw_implementation_specific_payload.hpp>
 
-#include <memory>
-
-rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr
-rclcpp::intra_process_manager::create_default_impl()
+namespace rclcpp
 {
-  return std::make_shared<IntraProcessManagerImpl<>>();
+namespace detail
+{
+
+bool
+RMWImplementationSpecificPayload::has_been_customized() const
+{
+  return nullptr != this->get_implementation_identifier();
 }
+
+const char *
+RMWImplementationSpecificPayload::get_implementation_identifier() const
+{
+  return nullptr;
+}
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp

@@ -0,0 +1,33 @@
+// 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.
+
+#include <rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp>
+
+#include "rcl/publisher.h"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+void
+RMWImplementationSpecificPublisherPayload::modify_rmw_publisher_options(
+  rmw_publisher_options_t & rmw_publisher_options) const
+{
+  // By default, do not mutate the rmw publisher options.
+  (void)rmw_publisher_options;
+}
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp

@@ -0,0 +1,33 @@
+// 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.
+
+#include <rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp>
+
+#include "rcl/subscription.h"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+void
+RMWImplementationSpecificSubscriptionPayload::modify_rmw_subscription_options(
+  rmw_subscription_options_t & rmw_subscription_options) const
+{
+  // By default, do not mutate the rmw subscription options.
+  (void)rmw_subscription_options;
+}
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/src/rclcpp/duration.cpp

@@ -233,4 +233,10 @@ Duration::to_rmw_time() const
   return result;
 }
 
+Duration
+Duration::from_seconds(double seconds)
+{
+  return Duration(static_cast<int64_t>(RCL_S_TO_NS(seconds)));
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/exceptions.cpp

@@ -17,6 +17,7 @@
 #include <cstdio>
 #include <functional>
 #include <string>
+#include <vector>
 
 using namespace std::string_literals;
 
@@ -68,6 +69,8 @@ from_rcl_error(
       return std::make_exception_ptr(RCLBadAlloc(base_exc));
     case RCL_RET_INVALID_ARGUMENT:
       return std::make_exception_ptr(RCLInvalidArgument(base_exc, formatted_prefix));
+    case RCL_RET_INVALID_ROS_ARGS:
+      return std::make_exception_ptr(RCLInvalidROSArgsError(base_exc, formatted_prefix));
     default:
       return std::make_exception_ptr(RCLError(base_exc, formatted_prefix));
   }
@@ -126,5 +129,18 @@ RCLInvalidArgument::RCLInvalidArgument(
 : RCLErrorBase(base_exc), std::invalid_argument(prefix + base_exc.formatted_message)
 {}
 
+RCLInvalidROSArgsError::RCLInvalidROSArgsError(
+  rcl_ret_t ret,
+  const rcl_error_state_t * error_state,
+  const std::string & prefix)
+: RCLInvalidROSArgsError(RCLErrorBase(ret, error_state), prefix)
+{}
+
+RCLInvalidROSArgsError::RCLInvalidROSArgsError(
+  const RCLErrorBase & base_exc,
+  const std::string & prefix)
+: RCLErrorBase(base_exc), std::runtime_error(prefix + base_exc.formatted_message)
+{}
+
 }  // namespace exceptions
 }  // namespace rclcpp

rclcpp/src/rclcpp/executor.cpp

@@ -140,6 +140,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
     }
   }
   // Add the node's notify condition to the guard condition handles
+  std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
   memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition());
 }
 
@@ -178,6 +179,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
       }
     }
   }
+  std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
   memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition());
 }
 
@@ -285,9 +287,6 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   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);
   }
@@ -329,6 +328,32 @@ Executor::execute_subscription(
       rcl_reset_error();
     }
     subscription->return_serialized_message(serialized_msg);
+  } else if (subscription->can_loan_messages()) {
+    void * loaned_msg = nullptr;
+    auto ret = rcl_take_loaned_message(
+      subscription->get_subscription_handle().get(),
+      &loaned_msg,
+      &message_info,
+      nullptr);
+    if (RCL_RET_OK == ret) {
+      subscription->handle_loaned_message(loaned_msg, message_info);
+    } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "take_loaned failed for subscription on topic '%s': %s",
+        subscription->get_topic_name(), rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+    ret = rcl_return_loaned_message_from_subscription(
+      subscription->get_subscription_handle().get(),
+      loaned_msg);
+    if (RCL_RET_OK != ret) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "return_loaned_message failed for subscription on topic '%s': %s",
+        subscription->get_topic_name(), rcl_get_error_string().str);
+    }
+    loaned_msg = nullptr;
   } else {
     std::shared_ptr<void> message = subscription->create_message();
     auto ret = rcl_take(
@@ -347,30 +372,6 @@ Executor::execute_subscription(
   }
 }
 
-void
-Executor::execute_intra_process_subscription(
-  rclcpp::SubscriptionBase::SharedPtr subscription)
-{
-  rcl_interfaces::msg::IntraProcessMessage ipm;
-  rmw_message_info_t message_info;
-  rcl_ret_t status = rcl_take(
-    subscription->get_intra_process_subscription_handle().get(),
-    &ipm,
-    &message_info,
-    nullptr);
-
-  if (status == RCL_RET_OK) {
-    message_info.from_intra_process = true;
-    subscription->handle_intra_process_message(ipm, message_info);
-  } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
-    RCUTILS_LOG_ERROR_NAMED(
-      "rclcpp",
-      "take failed for intra process subscription on topic '%s': %s",
-      subscription->get_topic_name(), rcl_get_error_string().str);
-    rcl_reset_error();
-  }
-}
-
 void
 Executor::execute_timer(
   rclcpp::TimerBase::SharedPtr timer)
@@ -423,43 +424,47 @@ Executor::execute_client(
 void
 Executor::wait_for_work(std::chrono::nanoseconds timeout)
 {
-  // Collect the subscriptions and timers to be waited on
-  memory_strategy_->clear_handles();
-  bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
+  {
+    std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
 
-  // Clean up any invalid nodes, if they were detected
-  if (has_invalid_weak_nodes) {
-    auto node_it = weak_nodes_.begin();
-    auto gc_it = guard_conditions_.begin();
-    while (node_it != weak_nodes_.end()) {
-      if (node_it->expired()) {
-        node_it = weak_nodes_.erase(node_it);
-        memory_strategy_->remove_guard_condition(*gc_it);
-        gc_it = guard_conditions_.erase(gc_it);
-      } else {
-        ++node_it;
-        ++gc_it;
+    // Collect the subscriptions and timers to be waited on
+    memory_strategy_->clear_handles();
+    bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
+
+    // Clean up any invalid nodes, if they were detected
+    if (has_invalid_weak_nodes) {
+      auto node_it = weak_nodes_.begin();
+      auto gc_it = guard_conditions_.begin();
+      while (node_it != weak_nodes_.end()) {
+        if (node_it->expired()) {
+          node_it = weak_nodes_.erase(node_it);
+          memory_strategy_->remove_guard_condition(*gc_it);
+          gc_it = guard_conditions_.erase(gc_it);
+        } else {
+          ++node_it;
+          ++gc_it;
+        }
       }
     }
-  }
-  // clear wait set
-  if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear wait set");
-  }
+    // clear wait set
+    if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
+      throw std::runtime_error("Couldn't clear wait set");
+    }
 
-  // The size of waitables are accounted for in size of the other entities
-  rcl_ret_t ret = rcl_wait_set_resize(
-    &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
-    memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
-    memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
-    memory_strategy_->number_of_ready_events());
-  if (RCL_RET_OK != ret) {
-    throw std::runtime_error(
-            std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
-  }
+    // The size of waitables are accounted for in size of the other entities
+    rcl_ret_t ret = rcl_wait_set_resize(
+      &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
+      memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
+      memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
+      memory_strategy_->number_of_ready_events());
+    if (RCL_RET_OK != ret) {
+      throw std::runtime_error(
+              std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
+    }
 
-  if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
-    throw std::runtime_error("Couldn't fill wait set");
+    if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
+      throw std::runtime_error("Couldn't fill wait set");
+    }
   }
   rcl_ret_t status =
     rcl_wait(&wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
@@ -511,54 +516,29 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
       if (!group) {
         continue;
       }
-      for (auto & weak_timer : group->get_timer_ptrs()) {
-        auto t = weak_timer.lock();
-        if (t == timer) {
-          return group;
-        }
+      auto timer_ref = group->find_timer_ptrs_if(
+        [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool {
+          return timer_ptr == timer;
+        });
+      if (timer_ref) {
+        return group;
       }
     }
   }
   return rclcpp::callback_group::CallbackGroup::SharedPtr();
 }
 
-void
-Executor::get_next_timer(AnyExecutable & 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->is_ready()) {
-          any_exec.timer = timer;
-          any_exec.callback_group = group;
-          node = get_node_by_group(group);
-          return;
-        }
-      }
-    }
-  }
-}
-
 bool
 Executor::get_next_ready_executable(AnyExecutable & any_executable)
 {
-  // Check the timers to see if there are any that are ready, if so return
-  get_next_timer(any_executable);
+  // Check the timers to see if there are any that are ready
+  memory_strategy_->get_next_timer(any_executable, weak_nodes_);
   if (any_executable.timer) {
     return true;
   }
   // Check the subscriptions to see if there are any that are ready
   memory_strategy_->get_next_subscription(any_executable, weak_nodes_);
-  if (any_executable.subscription || any_executable.subscription_intra_process) {
+  if (any_executable.subscription) {
     return true;
   }
   // Check the services to see if there are any that are ready

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -27,8 +27,11 @@ using rclcpp::executors::MultiThreadedExecutor;
 MultiThreadedExecutor::MultiThreadedExecutor(
   const rclcpp::executor::ExecutorArgs & args,
   size_t number_of_threads,
-  bool yield_before_execute)
-: executor::Executor(args), yield_before_execute_(yield_before_execute)
+  bool yield_before_execute,
+  std::chrono::nanoseconds next_exec_timeout)
+: executor::Executor(args),
+  yield_before_execute_(yield_before_execute),
+  next_exec_timeout_(next_exec_timeout)
 {
   number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
   if (number_of_threads_ == 0) {
@@ -77,12 +80,17 @@ MultiThreadedExecutor::run(size_t)
       if (!rclcpp::ok(this->context_) || !spinning.load()) {
         return;
       }
-      if (!get_next_executable(any_exec)) {
+      if (!get_next_executable(any_exec, next_exec_timeout_)) {
         continue;
       }
       if (any_exec.timer) {
         // Guard against multiple threads getting the same timer.
         if (scheduled_timers_.count(any_exec.timer) != 0) {
+          // Make sure that any_exec's callback group is reset before
+          // the lock is released.
+          if (any_exec.callback_group) {
+            any_exec.callback_group->can_be_taken_from().store(true);
+          }
           continue;
         }
         scheduled_timers_.insert(any_exec.timer);

rclcpp/src/rclcpp/init_options.cpp

@@ -42,7 +42,9 @@ InitOptions::InitOptions(const rcl_init_options_t & init_options)
 
 InitOptions::InitOptions(const InitOptions & other)
 : InitOptions(*other.get_rcl_init_options())
-{}
+{
+  shutdown_on_sigint = other.shutdown_on_sigint;
+}
 
 InitOptions &
 InitOptions::operator=(const InitOptions & other)
@@ -53,6 +55,7 @@ InitOptions::operator=(const InitOptions & other)
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options");
     }
+    this->shutdown_on_sigint = other.shutdown_on_sigint;
   }
   return *this;
 }

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -12,69 +12,153 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/experimental/intra_process_manager.hpp"
+
+#include <atomic>
+#include <memory>
+#include <mutex>
 
 namespace rclcpp
 {
-namespace intra_process_manager
+namespace experimental
 {
 
 static std::atomic<uint64_t> _next_unique_id {1};
 
-IntraProcessManager::IntraProcessManager(
-  rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr impl)
-: impl_(impl)
+IntraProcessManager::IntraProcessManager()
 {}
 
 IntraProcessManager::~IntraProcessManager()
 {}
 
 uint64_t
-IntraProcessManager::add_publisher(
-  rclcpp::PublisherBase::SharedPtr publisher,
-  size_t buffer_size)
+IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher)
 {
+  std::unique_lock<std::shared_timed_mutex> lock(mutex_);
+
   auto id = IntraProcessManager::get_next_unique_id();
-  size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size();
-  auto mrb = publisher->make_mapped_ring_buffer(size);
-  impl_->add_publisher(id, publisher, mrb, size);
-  if (!mrb) {
-    throw std::runtime_error("failed to create a mapped ring buffer");
+
+  publishers_[id].publisher = publisher;
+  publishers_[id].topic_name = publisher->get_topic_name();
+  publishers_[id].qos = publisher->get_actual_qos().get_rmw_qos_profile();
+
+  // Initialize the subscriptions storage for this publisher.
+  pub_to_subs_[id] = SplittedSubscriptions();
+
+  // create an entry for the publisher id and populate with already existing subscriptions
+  for (auto & pair : subscriptions_) {
+    if (can_communicate(publishers_[id], pair.second)) {
+      insert_sub_id_for_pub(pair.first, id, pair.second.use_take_shared_method);
+    }
   }
+
   return id;
 }
 
 uint64_t
-IntraProcessManager::add_subscription(
-  rclcpp::SubscriptionBase::SharedPtr subscription)
+IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr subscription)
 {
+  std::unique_lock<std::shared_timed_mutex> lock(mutex_);
+
   auto id = IntraProcessManager::get_next_unique_id();
-  impl_->add_subscription(id, subscription);
+
+  subscriptions_[id].subscription = subscription;
+  subscriptions_[id].topic_name = subscription->get_topic_name();
+  subscriptions_[id].qos = subscription->get_actual_qos();
+  subscriptions_[id].use_take_shared_method = subscription->use_take_shared_method();
+
+  // adds the subscription id to all the matchable publishers
+  for (auto & pair : publishers_) {
+    if (can_communicate(pair.second, subscriptions_[id])) {
+      insert_sub_id_for_pub(id, pair.first, subscriptions_[id].use_take_shared_method);
+    }
+  }
+
   return id;
 }
 
 void
 IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id)
 {
-  impl_->remove_subscription(intra_process_subscription_id);
+  std::unique_lock<std::shared_timed_mutex> lock(mutex_);
+
+  subscriptions_.erase(intra_process_subscription_id);
+
+  for (auto & pair : pub_to_subs_) {
+    pair.second.take_shared_subscriptions.erase(
+      std::remove(
+        pair.second.take_shared_subscriptions.begin(),
+        pair.second.take_shared_subscriptions.end(),
+        intra_process_subscription_id),
+      pair.second.take_shared_subscriptions.end());
+
+    pair.second.take_ownership_subscriptions.erase(
+      std::remove(
+        pair.second.take_ownership_subscriptions.begin(),
+        pair.second.take_ownership_subscriptions.end(),
+        intra_process_subscription_id),
+      pair.second.take_ownership_subscriptions.end());
+  }
 }
 
 void
 IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id)
 {
-  impl_->remove_publisher(intra_process_publisher_id);
+  std::unique_lock<std::shared_timed_mutex> lock(mutex_);
+
+  publishers_.erase(intra_process_publisher_id);
+  pub_to_subs_.erase(intra_process_publisher_id);
 }
 
 bool
 IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const
 {
-  return impl_->matches_any_publishers(id);
+  std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+
+  for (auto & publisher_pair : publishers_) {
+    auto publisher = publisher_pair.second.publisher.lock();
+    if (!publisher) {
+      continue;
+    }
+    if (*publisher.get() == id) {
+      return true;
+    }
+  }
+  return false;
 }
 
 size_t
 IntraProcessManager::get_subscription_count(uint64_t intra_process_publisher_id) const
 {
-  return impl_->get_subscription_count(intra_process_publisher_id);
+  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 get_subscription_count for invalid or no longer existing publisher id");
+    return 0;
+  }
+
+  auto count =
+    publisher_it->second.take_shared_subscriptions.size() +
+    publisher_it->second.take_ownership_subscriptions.size();
+
+  return count;
+}
+
+SubscriptionIntraProcessBase::SharedPtr
+IntraProcessManager::get_subscription_intra_process(uint64_t intra_process_subscription_id)
+{
+  std::shared_lock<std::shared_timed_mutex> lock(mutex_);
+
+  auto subscription_it = subscriptions_.find(intra_process_subscription_id);
+  if (subscription_it == subscriptions_.end()) {
+    return nullptr;
+  } else {
+    return subscription_it->second.subscription;
+  }
 }
 
 uint64_t
@@ -99,5 +183,45 @@ IntraProcessManager::get_next_unique_id()
   return next_id;
 }
 
-}  // namespace intra_process_manager
+void
+IntraProcessManager::insert_sub_id_for_pub(
+  uint64_t sub_id,
+  uint64_t pub_id,
+  bool use_take_shared_method)
+{
+  if (use_take_shared_method) {
+    pub_to_subs_[pub_id].take_shared_subscriptions.push_back(sub_id);
+  } else {
+    pub_to_subs_[pub_id].take_ownership_subscriptions.push_back(sub_id);
+  }
+}
+
+bool
+IntraProcessManager::can_communicate(
+  PublisherInfo pub_info,
+  SubscriptionInfo sub_info) const
+{
+  // publisher and subscription must be on the same topic
+  if (strcmp(pub_info.topic_name, sub_info.topic_name) != 0) {
+    return false;
+  }
+
+  // TODO(alsora): the following checks for qos compatibility should be provided by the RMW
+  // a reliable subscription can't be connected with a best effort publisher
+  if (
+    sub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE &&
+    pub_info.qos.reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT)
+  {
+    return false;
+  }
+
+  // a publisher and a subscription with different durability can't communicate
+  if (sub_info.qos.durability != pub_info.qos.durability) {
+    return false;
+  }
+
+  return true;
+}
+
+}  // namespace experimental
 }  // namespace rclcpp

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -32,16 +32,12 @@ MemoryStrategy::get_subscription_by_handle(
       if (!group) {
         continue;
       }
-      for (auto & weak_subscription : group->get_subscription_ptrs()) {
-        auto subscription = weak_subscription.lock();
-        if (subscription) {
-          if (subscription->get_subscription_handle() == subscriber_handle) {
-            return subscription;
-          }
-          if (subscription->get_intra_process_subscription_handle() == subscriber_handle) {
-            return subscription;
-          }
-        }
+      auto match_subscription = group->find_subscription_ptrs_if(
+        [&subscriber_handle](const rclcpp::SubscriptionBase::SharedPtr & subscription) -> bool {
+          return subscription->get_subscription_handle() == subscriber_handle;
+        });
+      if (match_subscription) {
+        return match_subscription;
       }
     }
   }
@@ -63,11 +59,12 @@ MemoryStrategy::get_service_by_handle(
       if (!group) {
         continue;
       }
-      for (auto & weak_service : group->get_service_ptrs()) {
-        auto service = weak_service.lock();
-        if (service && service->get_service_handle() == service_handle) {
-          return service;
-        }
+      auto service_ref = group->find_service_ptrs_if(
+        [&service_handle](const rclcpp::ServiceBase::SharedPtr & service) -> bool {
+          return service->get_service_handle() == service_handle;
+        });
+      if (service_ref) {
+        return service_ref;
       }
     }
   }
@@ -89,11 +86,39 @@ MemoryStrategy::get_client_by_handle(
       if (!group) {
         continue;
       }
-      for (auto & weak_client : group->get_client_ptrs()) {
-        auto client = weak_client.lock();
-        if (client && client->get_client_handle() == client_handle) {
-          return client;
-        }
+      auto client_ref = group->find_client_ptrs_if(
+        [&client_handle](const rclcpp::ClientBase::SharedPtr & client) -> bool {
+          return client->get_client_handle() == client_handle;
+        });
+      if (client_ref) {
+        return client_ref;
+      }
+    }
+  }
+  return nullptr;
+}
+
+rclcpp::TimerBase::SharedPtr
+MemoryStrategy::get_timer_by_handle(
+  std::shared_ptr<const rcl_timer_t> timer_handle,
+  const WeakNodeList & 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;
+      }
+      auto timer_ref = group->find_timer_ptrs_if(
+        [&timer_handle](const rclcpp::TimerBase::SharedPtr & timer) -> bool {
+          return timer->get_timer_handle() == timer_handle;
+        });
+      if (timer_ref) {
+        return timer_ref;
       }
     }
   }
@@ -138,11 +163,12 @@ MemoryStrategy::get_group_by_subscription(
       if (!group) {
         continue;
       }
-      for (auto & weak_sub : group->get_subscription_ptrs()) {
-        auto sub = weak_sub.lock();
-        if (sub == subscription) {
-          return group;
-        }
+      auto match_sub = group->find_subscription_ptrs_if(
+        [&subscription](const rclcpp::SubscriptionBase::SharedPtr & sub) -> bool {
+          return sub == subscription;
+        });
+      if (match_sub) {
+        return group;
       }
     }
   }
@@ -164,11 +190,12 @@ MemoryStrategy::get_group_by_service(
       if (!group) {
         continue;
       }
-      for (auto & weak_serv : group->get_service_ptrs()) {
-        auto serv = weak_serv.lock();
-        if (serv && serv == service) {
-          return group;
-        }
+      auto service_ref = group->find_service_ptrs_if(
+        [&service](const rclcpp::ServiceBase::SharedPtr & serv) -> bool {
+          return serv == service;
+        });
+      if (service_ref) {
+        return group;
       }
     }
   }
@@ -190,11 +217,39 @@ MemoryStrategy::get_group_by_client(
       if (!group) {
         continue;
       }
-      for (auto & weak_client : group->get_client_ptrs()) {
-        auto cli = weak_client.lock();
-        if (cli && cli == client) {
-          return group;
-        }
+      auto client_ref = group->find_client_ptrs_if(
+        [&client](const rclcpp::ClientBase::SharedPtr & cli) -> bool {
+          return cli == client;
+        });
+      if (client_ref) {
+        return group;
+      }
+    }
+  }
+  return nullptr;
+}
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+MemoryStrategy::get_group_by_timer(
+  rclcpp::TimerBase::SharedPtr timer,
+  const WeakNodeList & 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;
+      }
+      auto timer_ref = group->find_timer_ptrs_if(
+        [&timer](const rclcpp::TimerBase::SharedPtr & time) -> bool {
+          return time == timer;
+        });
+      if (timer_ref) {
+        return group;
       }
     }
   }
@@ -216,11 +271,12 @@ MemoryStrategy::get_group_by_waitable(
       if (!group) {
         continue;
       }
-      for (auto & weak_waitable : group->get_waitable_ptrs()) {
-        auto group_waitable = weak_waitable.lock();
-        if (group_waitable && group_waitable == waitable) {
-          return group;
-        }
+      auto waitable_ref = group->find_waitable_ptrs_if(
+        [&waitable](const rclcpp::Waitable::SharedPtr & group_waitable) -> bool {
+          return group_waitable == waitable;
+        });
+      if (waitable_ref) {
+        return group;
       }
     }
   }

rclcpp/src/rclcpp/node.cpp

@@ -27,17 +27,7 @@
 #include "rclcpp/node_interfaces/node_clock.hpp"
 #include "rclcpp/node_interfaces/node_graph.hpp"
 #include "rclcpp/node_interfaces/node_logging.hpp"
-// When compiling this file, Windows produces a deprecation warning for the
-// deprecated function prototype of NodeParameters::register_param_change_callback().
-// Other compilers do not.
-#if defined(_WIN32)
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
 #include "rclcpp/node_interfaces/node_parameters.hpp"
-#if defined(_WIN32)
-# pragma warning(pop)
-#endif
 #include "rclcpp/node_interfaces/node_services.hpp"
 #include "rclcpp/node_interfaces/node_time_source.hpp"
 #include "rclcpp/node_interfaces/node_timers.hpp"
@@ -236,9 +226,14 @@ const rclcpp::ParameterValue &
 Node::declare_parameter(
   const std::string & name,
   const rclcpp::ParameterValue & default_value,
-  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor)
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
+  bool ignore_override)
 {
-  return this->node_parameters_->declare_parameter(name, default_value, parameter_descriptor);
+  return this->node_parameters_->declare_parameter(
+    name,
+    default_value,
+    parameter_descriptor,
+    ignore_override);
 }
 
 void
@@ -321,6 +316,18 @@ Node::list_parameters(const std::vector<std::string> & prefixes, uint64_t depth)
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
+rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr
+Node::add_on_set_parameters_callback(OnParametersSetCallbackType callback)
+{
+  return node_parameters_->add_on_set_parameters_callback(callback);
+}
+
+void
+Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback)
+{
+  return node_parameters_->remove_on_set_parameters_callback(callback);
+}
+
 rclcpp::Node::OnParametersSetCallbackType
 Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback)
 {

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -137,7 +137,8 @@ NodeGraph::get_node_names() const
   std::vector<std::string> nodes;
   auto names_and_namespaces = get_node_names_and_namespaces();
 
-  std::transform(names_and_namespaces.begin(),
+  std::transform(
+    names_and_namespaces.begin(),
     names_and_namespaces.end(),
     std::back_inserter(nodes),
     [](std::pair<std::string, std::string> nns) {

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -12,22 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// When compiling this file, Windows produces a deprecation warning for the
-// deprecated function prototype of NodeParameters::register_param_change_callback().
-// Other compilers do not.
-#if defined(_WIN32)
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
 #include "rclcpp/node_interfaces/node_parameters.hpp"
-#if defined(_WIN32)
-# pragma warning(pop)
-#endif
 
 #include <rcl_yaml_param_parser/parser.h>
 
 #include <cmath>
 #include <cstdlib>
+#include <functional>
 #include <limits>
 #include <map>
 #include <memory>
@@ -93,66 +84,39 @@ NodeParameters::NodeParameters(
     throw std::runtime_error("Need valid node options in NodeParameters");
   }
 
-  // Get paths to yaml files containing initial parameter values
-  std::vector<std::string> yaml_paths;
-
-  auto get_yaml_paths = [&yaml_paths, &options](const rcl_arguments_t * args) {
-      int num_yaml_files = rcl_arguments_get_param_files_count(args);
-      if (num_yaml_files > 0) {
-        char ** param_files;
-        rcl_ret_t ret = rcl_arguments_get_param_files(args, options->allocator, &param_files);
-        if (RCL_RET_OK != ret) {
-          rclcpp::exceptions::throw_from_rcl_error(ret);
-        }
-        auto cleanup_param_files = make_scope_exit(
-          [&param_files, &num_yaml_files, &options]() {
-            for (int i = 0; i < num_yaml_files; ++i) {
-              options->allocator.deallocate(param_files[i], options->allocator.state);
-            }
-            options->allocator.deallocate(param_files, options->allocator.state);
-          });
-        for (int i = 0; i < num_yaml_files; ++i) {
-          yaml_paths.emplace_back(param_files[i]);
-        }
-      }
-    };
-
+  std::vector<const rcl_arguments_t *> argument_sources;
   // global before local so that local overwrites global
   if (options->use_global_arguments) {
     auto context_ptr = node_base->get_context()->get_rcl_context();
-    get_yaml_paths(&(context_ptr->global_arguments));
+    argument_sources.push_back(&(context_ptr->global_arguments));
   }
-  get_yaml_paths(&(options->arguments));
+  argument_sources.push_back(&options->arguments);
 
   // Get fully qualified node name post-remapping to use to find node's params in yaml files
   combined_name_ = node_base->get_fully_qualified_name();
 
-  // TODO(sloretz) use rcl to parse yaml when circular dependency is solved
-  // See https://github.com/ros2/rcl/issues/252
-  for (const std::string & yaml_path : yaml_paths) {
-    rcl_params_t * yaml_params = rcl_yaml_node_struct_init(options->allocator);
-    if (nullptr == yaml_params) {
-      throw std::bad_alloc();
+  for (const rcl_arguments_t * source : argument_sources) {
+    rcl_params_t * params = NULL;
+    rcl_ret_t ret = rcl_arguments_get_param_overrides(source, &params);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
     }
-    if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params)) {
-      std::ostringstream ss;
-      ss << "Failed to parse parameters from file '" << yaml_path << "': " <<
-        rcl_get_error_string().str;
-      rcl_reset_error();
-      throw std::runtime_error(ss.str());
-    }
-
-    rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(yaml_params);
-    rcl_yaml_node_struct_fini(yaml_params);
-    auto iter = initial_map.find(combined_name_);
-    if (initial_map.end() == iter) {
-      continue;
-    }
-
-    // Combine parameter yaml files, overwriting values in older ones
-    for (auto & param : iter->second) {
-      parameter_overrides_[param.get_name()] =
-        rclcpp::ParameterValue(param.get_value_message());
+    if (params) {
+      auto cleanup_params = make_scope_exit(
+        [params]() {
+          rcl_yaml_node_struct_fini(params);
+        });
+      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params);
+      for (auto iter = initial_map.begin(); initial_map.end() != iter; iter++) {
+        // TODO(cottsay) implement further wildcard matching
+        if (iter->first == "/**" || iter->first == combined_name_) {
+          // Combine parameter yaml files, overwriting values in older ones
+          for (auto & param : iter->second) {
+            parameter_overrides_[param.get_name()] =
+              rclcpp::ParameterValue(param.get_value_message());
+          }
+        }
+      }
     }
   }
 
@@ -170,7 +134,8 @@ NodeParameters::NodeParameters(
         this->declare_parameter(
           pair.first,
           pair.second,
-          rcl_interfaces::msg::ParameterDescriptor());
+          rcl_interfaces::msg::ParameterDescriptor(),
+          true);
       }
     }
   }
@@ -239,9 +204,7 @@ __check_parameter_value_in_range(
   if (!descriptor.floating_point_range.empty() && value.get_type() == rclcpp::PARAMETER_DOUBLE) {
     double v = value.get<double>();
     auto fp_range = descriptor.floating_point_range.at(0);
-    if (__are_doubles_equal(v, fp_range.from_value) ||
-      __are_doubles_equal(v, fp_range.to_value))
-    {
+    if (__are_doubles_equal(v, fp_range.from_value) || __are_doubles_equal(v, fp_range.to_value)) {
       return result;
     }
     if ((v < fp_range.from_value) || (v > fp_range.to_value)) {
@@ -287,20 +250,50 @@ __check_parameters(
 
 using OnParametersSetCallbackType =
   rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
+using CallbacksContainerType =
+  rclcpp::node_interfaces::NodeParameters::CallbacksContainerType;
+using OnSetParametersCallbackHandle =
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle;
+
+RCLCPP_LOCAL
+rcl_interfaces::msg::SetParametersResult
+__call_on_parameters_set_callbacks(
+  const std::vector<rclcpp::Parameter> & parameters,
+  CallbacksContainerType & callback_container,
+  const OnParametersSetCallbackType & callback)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  auto it = callback_container.begin();
+  while (it != callback_container.end()) {
+    auto shared_handle = it->lock();
+    if (nullptr != shared_handle) {
+      result = shared_handle->callback(parameters);
+      if (!result.successful) {
+        return result;
+      }
+      it++;
+    } else {
+      it = callback_container.erase(it);
+    }
+  }
+  if (callback) {
+    result = callback(parameters);
+  }
+  return result;
+}
 
 RCLCPP_LOCAL
 rcl_interfaces::msg::SetParametersResult
 __set_parameters_atomically_common(
   const std::vector<rclcpp::Parameter> & parameters,
   std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameter_infos,
-  OnParametersSetCallbackType on_set_parameters_callback)
+  CallbacksContainerType & callback_container,
+  const OnParametersSetCallbackType & callback)
 {
   // Call the user callback to see if the new value(s) are allowed.
-  rcl_interfaces::msg::SetParametersResult result;
-  result.successful = true;
-  if (on_set_parameters_callback) {
-    result = on_set_parameters_callback(parameters);
-  }
+  rcl_interfaces::msg::SetParametersResult result =
+    __call_on_parameters_set_callbacks(parameters, callback_container, callback);
   if (!result.successful) {
     return result;
   }
@@ -330,19 +323,21 @@ __declare_parameter_common(
   const rclcpp::ParameterValue & default_value,
   const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
   std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameters_out,
-  const std::map<std::string, rclcpp::ParameterValue> & initial_values,
-  OnParametersSetCallbackType on_set_parameters_callback,
-  rcl_interfaces::msg::ParameterEvent * parameter_event_out)
+  const std::map<std::string, rclcpp::ParameterValue> & overrides,
+  CallbacksContainerType & callback_container,
+  const OnParametersSetCallbackType & callback,
+  rcl_interfaces::msg::ParameterEvent * parameter_event_out,
+  bool ignore_override = false)
 {
   using rclcpp::node_interfaces::ParameterInfo;
   std::map<std::string, ParameterInfo> parameter_infos {{name, ParameterInfo()}};
   parameter_infos.at(name).descriptor = parameter_descriptor;
 
-  // Use the value from the initial_values if available, otherwise use the default.
+  // Use the value from the overrides if available, otherwise use the default.
   const rclcpp::ParameterValue * initial_value = &default_value;
-  auto initial_value_it = initial_values.find(name);
-  if (initial_value_it != initial_values.end()) {
-    initial_value = &initial_value_it->second;
+  auto overrides_it = overrides.find(name);
+  if (!ignore_override && overrides_it != overrides.end()) {
+    initial_value = &overrides_it->second;
   }
 
   // Check with the user's callback to see if the initial value can be set.
@@ -351,7 +346,8 @@ __declare_parameter_common(
   auto result = __set_parameters_atomically_common(
     parameter_wrappers,
     parameter_infos,
-    on_set_parameters_callback);
+    callback_container,
+    callback);
 
   // Add declared parameters to storage.
   parameters_out[name] = parameter_infos.at(name);
@@ -368,9 +364,12 @@ const rclcpp::ParameterValue &
 NodeParameters::declare_parameter(
   const std::string & name,
   const rclcpp::ParameterValue & default_value,
-  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor)
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
+  bool ignore_override)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
 
   // TODO(sloretz) parameter name validation
   if (name.empty()) {
@@ -390,8 +389,10 @@ NodeParameters::declare_parameter(
     parameter_descriptor,
     parameters_,
     parameter_overrides_,
+    on_parameters_set_callback_container_,
     on_parameters_set_callback_,
-    &parameter_event);
+    &parameter_event,
+    ignore_override);
 
   // If it failed to be set, then throw an exception.
   if (!result.successful) {
@@ -401,6 +402,8 @@ NodeParameters::declare_parameter(
 
   // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor.
   if (nullptr != events_publisher_) {
+    parameter_event.node = combined_name_;
+    parameter_event.stamp = node_clock_->get_clock()->now();
     events_publisher_->publish(parameter_event);
   }
 
@@ -410,7 +413,9 @@ NodeParameters::declare_parameter(
 void
 NodeParameters::undeclare_parameter(const std::string & name)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
 
   auto parameter_info = parameters_.find(name);
   if (parameter_info == parameters_.end()) {
@@ -429,7 +434,7 @@ NodeParameters::undeclare_parameter(const std::string & name)
 bool
 NodeParameters::has_parameter(const std::string & name) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
 
   return __lockless_has_parameter(parameters_, name);
 }
@@ -463,7 +468,9 @@ __find_parameter_by_name(
 rcl_interfaces::msg::SetParametersResult
 NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
 
   rcl_interfaces::msg::SetParametersResult result;
 
@@ -512,6 +519,7 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
   std::map<std::string, rclcpp::node_interfaces::ParameterInfo> staged_parameter_changes;
   rcl_interfaces::msg::ParameterEvent parameter_event_msg;
   parameter_event_msg.node = combined_name_;
+  CallbacksContainerType empty_callback_container;
   for (auto parameter_to_be_declared : parameters_to_be_declared) {
     // This should not throw, because we validated the name and checked that
     // the parameter was not already declared.
@@ -521,8 +529,11 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
       rcl_interfaces::msg::ParameterDescriptor(),  // Implicit declare uses default descriptor.
       staged_parameter_changes,
       parameter_overrides_,
-      nullptr,  // callback is explicitly null, so that it is called only once, when setting below.
-      &parameter_event_msg);
+      // Only call callbacks once below
+      empty_callback_container,  // callback_container is explicitly empty
+      nullptr,  // callback is explicitly null.
+      &parameter_event_msg,
+      true);
     if (!result.successful) {
       // Declare failed, return knowing that nothing was changed because the
       // staged changes were not applied.
@@ -569,13 +580,16 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
     }
   }
 
-  // Set the all of the parameters including the ones declared implicitly above.
+  // Set all of the parameters including the ones declared implicitly above.
   result = __set_parameters_atomically_common(
     // either the original parameters given by the user, or ones updated with initial values
     *parameters_to_be_set,
     // they are actually set on the official parameter storage
     parameters_,
     // this will get called once, with all the parameters to be set
+    on_parameters_set_callback_container_,
+    // These callbacks are called once. When a callback returns an unsuccessful result,
+    // the remaining aren't called.
     on_parameters_set_callback_);
 
   // If not successful, then stop here.
@@ -638,7 +652,7 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
 std::vector<rclcpp::Parameter>
 NodeParameters::get_parameters(const std::vector<std::string> & names) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
   std::vector<rclcpp::Parameter> results;
   results.reserve(names.size());
 
@@ -677,7 +691,7 @@ NodeParameters::get_parameter(
   const std::string & name,
   rclcpp::Parameter & parameter) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
 
   auto param_iter = parameters_.find(name);
   if (
@@ -696,7 +710,7 @@ NodeParameters::get_parameters_by_prefix(
   const std::string & prefix,
   std::map<std::string, rclcpp::Parameter> & parameters) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
 
   std::string prefix_with_dot = prefix.empty() ? prefix : prefix + ".";
   bool ret = false;
@@ -715,7 +729,7 @@ NodeParameters::get_parameters_by_prefix(
 std::vector<rcl_interfaces::msg::ParameterDescriptor>
 NodeParameters::describe_parameters(const std::vector<std::string> & names) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
   std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
   results.reserve(names.size());
 
@@ -743,7 +757,7 @@ NodeParameters::describe_parameters(const std::vector<std::string> & names) cons
 std::vector<uint8_t>
 NodeParameters::get_parameter_types(const std::vector<std::string> & names) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
   std::vector<uint8_t> results;
   results.reserve(names.size());
 
@@ -769,7 +783,7 @@ NodeParameters::get_parameter_types(const std::vector<std::string> & names) cons
 rcl_interfaces::msg::ListParametersResult
 NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
   rcl_interfaces::msg::ListParametersResult result;
 
   // TODO(mikaelarguedas) define parameter separator different from "/" to avoid ambiguity
@@ -779,25 +793,27 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
     bool get_all = (prefixes.size() == 0) &&
       ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
       (static_cast<uint64_t>(std::count(kv.first.begin(), kv.first.end(), *separator)) < depth));
-    bool prefix_matches = std::any_of(prefixes.cbegin(), prefixes.cend(),
-        [&kv, &depth, &separator](const std::string & prefix) {
-          if (kv.first == prefix) {
-            return true;
-          } else if (kv.first.find(prefix + separator) == 0) {
-            size_t length = prefix.length();
-            std::string substr = kv.first.substr(length);
-            // Cast as unsigned integer to avoid warning
-            return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
-            (static_cast<uint64_t>(std::count(substr.begin(), substr.end(), *separator)) < depth);
-          }
-          return false;
-        });
+    bool prefix_matches = std::any_of(
+      prefixes.cbegin(), prefixes.cend(),
+      [&kv, &depth, &separator](const std::string & prefix) {
+        if (kv.first == prefix) {
+          return true;
+        } else if (kv.first.find(prefix + separator) == 0) {
+          size_t length = prefix.length();
+          std::string substr = kv.first.substr(length);
+          // Cast as unsigned integer to avoid warning
+          return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
+          (static_cast<uint64_t>(std::count(substr.begin(), substr.end(), *separator)) < depth);
+        }
+        return false;
+      });
     if (get_all || prefix_matches) {
       result.names.push_back(kv.first);
       size_t last_separator = kv.first.find_last_of(separator);
       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) ==
+        if (
+          std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
           result.prefixes.cend())
         {
           result.prefixes.push_back(prefix);
@@ -808,37 +824,65 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
   return result;
 }
 
+struct HandleCompare
+  : public std::unary_function<OnSetParametersCallbackHandle::WeakPtr, bool>
+{
+  explicit HandleCompare(const OnSetParametersCallbackHandle * const base)
+  : base_(base) {}
+  bool operator()(const OnSetParametersCallbackHandle::WeakPtr & handle)
+  {
+    auto shared_handle = handle.lock();
+    if (base_ == shared_handle.get()) {
+      return true;
+    }
+    return false;
+  }
+  const OnSetParametersCallbackHandle * const base_;
+};
+
+void
+NodeParameters::remove_on_set_parameters_callback(
+  const OnSetParametersCallbackHandle * const handle)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  auto it = std::find_if(
+    on_parameters_set_callback_container_.begin(),
+    on_parameters_set_callback_container_.end(),
+    HandleCompare(handle));
+  if (it != on_parameters_set_callback_container_.end()) {
+    on_parameters_set_callback_container_.erase(it);
+  } else {
+    throw std::runtime_error("Callback doesn't exist");
+  }
+}
+
+OnSetParametersCallbackHandle::SharedPtr
+NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callback)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  auto handle = std::make_shared<OnSetParametersCallbackHandle>();
+  handle->callback = callback;
+  // the last callback registered is executed first.
+  on_parameters_set_callback_container_.emplace_front(handle);
+  return handle;
+}
+
 NodeParameters::OnParametersSetCallbackType
 NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback)
 {
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
   auto existing_callback = on_parameters_set_callback_;
   on_parameters_set_callback_ = callback;
   return existing_callback;
 }
 
-#if !defined(_WIN32)
-# pragma GCC diagnostic push
-# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#else  // !defined(_WIN32)
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
-void
-NodeParameters::register_param_change_callback(ParametersCallbackFunction callback)
-{
-  if (on_parameters_set_callback_) {
-    RCLCPP_WARN(
-      node_logging_->get_logger(),
-      "on_parameters_set_callback already registered, overwriting previous callback");
-  }
-  on_parameters_set_callback_ = callback;
-}
-#if !defined(_WIN32)
-# pragma GCC diagnostic pop
-#else  // !defined(_WIN32)
-# pragma warning(pop)
-#endif
-
 const std::map<std::string, rclcpp::ParameterValue> &
 NodeParameters::get_parameter_overrides() const
 {

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -16,7 +16,6 @@
 
 #include <string>
 
-#include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/exceptions.hpp"
 
 using rclcpp::exceptions::throw_from_rcl_error;
@@ -34,36 +33,10 @@ rclcpp::PublisherBase::SharedPtr
 NodeTopics::create_publisher(
   const std::string & topic_name,
   const rclcpp::PublisherFactory & publisher_factory,
-  const rcl_publisher_options_t & publisher_options,
-  bool use_intra_process)
+  const rclcpp::QoS & qos)
 {
-  // Create the MessageT specific Publisher using the factory, but store it as PublisherBase.
-  auto publisher = publisher_factory.create_typed_publisher(
-    node_base_, topic_name, publisher_options);
-
-  // Setup intra process publishing if requested.
-  if (use_intra_process) {
-    auto context = node_base_->get_context();
-    // Get the intra process manager instance for this context.
-    auto ipm = context->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
-    // Register the publisher with the intra process manager.
-    if (publisher_options.qos.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL) {
-      throw std::invalid_argument(
-              "intraprocess communication is not allowed with keep all history qos policy");
-    }
-    if (publisher_options.qos.depth == 0) {
-      throw std::invalid_argument(
-              "intraprocess communication is not allowed with a zero qos history depth value");
-    }
-    uint64_t intra_process_publisher_id = ipm->add_publisher(publisher);
-    publisher->setup_intra_process(
-      intra_process_publisher_id,
-      ipm,
-      publisher_options);
-  }
-
-  // Return the completed publisher.
-  return publisher;
+  // Create the MessageT specific Publisher using the factory, but return it as PublisherBase.
+  return publisher_factory.create_typed_publisher(node_base_, topic_name, qos);
 }
 
 void
@@ -99,25 +72,10 @@ rclcpp::SubscriptionBase::SharedPtr
 NodeTopics::create_subscription(
   const std::string & topic_name,
   const rclcpp::SubscriptionFactory & subscription_factory,
-  const rcl_subscription_options_t & subscription_options,
-  bool use_intra_process)
+  const rclcpp::QoS & qos)
 {
-  auto subscription = subscription_factory.create_typed_subscription(
-    node_base_, topic_name, subscription_options);
-
-  // Setup intra process publishing if requested.
-  if (use_intra_process) {
-    auto context = node_base_->get_context();
-    auto ipm =
-      context->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
-    uint64_t intra_process_subscription_id = ipm->add_subscription(subscription);
-    auto options_copy = subscription_options;
-    options_copy.ignore_local_publications = false;
-    subscription->setup_intra_process(intra_process_subscription_id, ipm, options_copy);
-  }
-
-  // Return the completed subscription.
-  return subscription;
+  // Create the MessageT specific Subscription using the factory, but return a SubscriptionBase.
+  return subscription_factory.create_typed_subscription(node_base_, topic_name, qos);
 }
 
 void
@@ -136,18 +94,24 @@ NodeTopics::add_subscription(
   }
 
   callback_group->add_subscription(subscription);
+
   for (auto & subscription_event : subscription->get_event_handlers()) {
     callback_group->add_waitable(subscription_event);
   }
 
+  auto intra_process_waitable = subscription->get_intra_process_waitable();
+  if (nullptr != intra_process_waitable) {
+    // Add to the callback group to be notified about intra-process msgs.
+    callback_group->add_waitable(intra_process_waitable);
+  }
+
   // Notify the executor that a new subscription was created using the parent Node.
   {
     auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
-    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
-      throw std::runtime_error(
-              std::string("Failed to notify wait set on subscription creation: ") +
-              rmw_get_error_string().str
-      );
+    auto ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition());
+    if (ret != RCL_RET_OK) {
+      using rclcpp::exceptions::throw_from_rcl_error;
+      throw_from_rcl_error(ret, "failed to notify wait set on subscription creation");
     }
   }
 }

rclcpp/src/rclcpp/node_options.cpp

@@ -18,6 +18,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <utility>
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logging.hpp"
@@ -90,25 +91,48 @@ NodeOptions::get_rcl_node_options() const
     node_options_->use_global_arguments = this->use_global_arguments_;
     node_options_->domain_id = this->get_domain_id_from_env();
 
-    std::unique_ptr<const char *[]> c_args;
+    int c_argc = 0;
+    std::unique_ptr<const char *[]> c_argv;
     if (!this->arguments_.empty()) {
-      c_args.reset(new const char *[this->arguments_.size()]);
+      if (this->arguments_.size() > static_cast<size_t>(std::numeric_limits<int>::max())) {
+        throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args");
+      }
+
+      c_argc = static_cast<int>(this->arguments_.size());
+      c_argv.reset(new const char *[c_argc]);
+
       for (std::size_t i = 0; i < this->arguments_.size(); ++i) {
-        c_args[i] = this->arguments_[i].c_str();
+        c_argv[i] = this->arguments_[i].c_str();
       }
     }
 
-    if (this->arguments_.size() > std::numeric_limits<int>::max()) {
-      throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args");
-    }
-
-    rmw_ret_t ret = rcl_parse_arguments(
-      static_cast<int>(this->arguments_.size()), c_args.get(), this->allocator_,
-      &(node_options_->arguments));
+    rcl_ret_t ret = rcl_parse_arguments(
+      c_argc, c_argv.get(), this->allocator_, &(node_options_->arguments));
 
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to parse arguments");
     }
+
+    int unparsed_ros_args_count =
+      rcl_arguments_get_count_unparsed_ros(&(node_options_->arguments));
+    if (unparsed_ros_args_count > 0) {
+      int * unparsed_ros_args_indices = nullptr;
+      ret = rcl_arguments_get_unparsed_ros(
+        &(node_options_->arguments), this->allocator_, &unparsed_ros_args_indices);
+      if (RCL_RET_OK != ret) {
+        throw_from_rcl_error(ret, "failed to get unparsed ROS arguments");
+      }
+      try {
+        std::vector<std::string> unparsed_ros_args;
+        for (int i = 0; i < unparsed_ros_args_count; ++i) {
+          unparsed_ros_args.push_back(c_argv[unparsed_ros_args_indices[i]]);
+        }
+        throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_args));
+      } catch (...) {
+        this->allocator_.deallocate(unparsed_ros_args_indices, this->allocator_.state);
+        throw;
+      }
+    }
   }
 
   return node_options_.get();

rclcpp/src/rclcpp/parameter.cpp

@@ -19,17 +19,7 @@
 #include <string>
 #include <vector>
 
-// When compiling this file, Windows produces a deprecation warning for the
-// deprecated function prototype of NodeParameters::register_param_change_callback().
-// Other compilers do not.
-#if defined(_WIN32)
-# pragma warning(push)
-# pragma warning(disable: 4996)
-#endif
 #include "rclcpp/node_interfaces/node_parameters.hpp"
-#if defined(_WIN32)
-# pragma warning(pop)
-#endif
 #include "rclcpp/utilities.hpp"
 
 using rclcpp::ParameterType;

rclcpp/src/rclcpp/parameter_client.cpp

@@ -30,7 +30,8 @@ AsyncParametersClient::AsyncParametersClient(
   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)
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
 : node_topics_interface_(node_topics_interface)
 {
   if (remote_node_name != "") {
@@ -51,7 +52,7 @@ AsyncParametersClient::AsyncParametersClient(
     remote_node_name_ + "/" + parameter_service_names::get_parameters,
     options);
   auto get_parameters_base = std::dynamic_pointer_cast<ClientBase>(get_parameters_client_);
-  node_services_interface->add_client(get_parameters_base, nullptr);
+  node_services_interface->add_client(get_parameters_base, group);
 
   get_parameter_types_client_ = Client<rcl_interfaces::srv::GetParameterTypes>::make_shared(
     node_base_interface.get(),
@@ -60,7 +61,7 @@ AsyncParametersClient::AsyncParametersClient(
     options);
   auto get_parameter_types_base =
     std::dynamic_pointer_cast<ClientBase>(get_parameter_types_client_);
-  node_services_interface->add_client(get_parameter_types_base, nullptr);
+  node_services_interface->add_client(get_parameter_types_base, group);
 
   set_parameters_client_ = Client<rcl_interfaces::srv::SetParameters>::make_shared(
     node_base_interface.get(),
@@ -68,16 +69,17 @@ AsyncParametersClient::AsyncParametersClient(
     remote_node_name_ + "/" + parameter_service_names::set_parameters,
     options);
   auto set_parameters_base = std::dynamic_pointer_cast<ClientBase>(set_parameters_client_);
-  node_services_interface->add_client(set_parameters_base, nullptr);
+  node_services_interface->add_client(set_parameters_base, group);
 
   set_parameters_atomically_client_ =
-    Client<rcl_interfaces::srv::SetParametersAtomically>::make_shared(node_base_interface.get(),
-      node_graph_interface,
-      remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically,
-      options);
+    Client<rcl_interfaces::srv::SetParametersAtomically>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically,
+    options);
   auto set_parameters_atomically_base = std::dynamic_pointer_cast<ClientBase>(
     set_parameters_atomically_client_);
-  node_services_interface->add_client(set_parameters_atomically_base, nullptr);
+  node_services_interface->add_client(set_parameters_atomically_base, group);
 
   list_parameters_client_ = Client<rcl_interfaces::srv::ListParameters>::make_shared(
     node_base_interface.get(),
@@ -85,7 +87,7 @@ AsyncParametersClient::AsyncParametersClient(
     remote_node_name_ + "/" + parameter_service_names::list_parameters,
     options);
   auto list_parameters_base = std::dynamic_pointer_cast<ClientBase>(list_parameters_client_);
-  node_services_interface->add_client(list_parameters_base, nullptr);
+  node_services_interface->add_client(list_parameters_base, group);
 
   describe_parameters_client_ = Client<rcl_interfaces::srv::DescribeParameters>::make_shared(
     node_base_interface.get(),
@@ -94,33 +96,37 @@ AsyncParametersClient::AsyncParametersClient(
     options);
   auto describe_parameters_base =
     std::dynamic_pointer_cast<ClientBase>(describe_parameters_client_);
-  node_services_interface->add_client(describe_parameters_base, nullptr);
+  node_services_interface->add_client(describe_parameters_base, group);
 }
 
 AsyncParametersClient::AsyncParametersClient(
   const rclcpp::Node::SharedPtr node,
   const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
 : AsyncParametersClient(
     node->get_node_base_interface(),
     node->get_node_topics_interface(),
     node->get_node_graph_interface(),
     node->get_node_services_interface(),
     remote_node_name,
-    qos_profile)
+    qos_profile,
+    group)
 {}
 
 AsyncParametersClient::AsyncParametersClient(
   rclcpp::Node * node,
   const std::string & remote_node_name,
-  const rmw_qos_profile_t & qos_profile)
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
 : AsyncParametersClient(
     node->get_node_base_interface(),
     node->get_node_topics_interface(),
     node->get_node_graph_interface(),
     node->get_node_services_interface(),
     remote_node_name,
-    qos_profile)
+    qos_profile,
+    group)
 {}
 
 std::shared_future<std::vector<rclcpp::Parameter>>
@@ -150,8 +156,7 @@ AsyncParametersClient::get_parameters(
         rcl_interfaces::msg::Parameter parameter;
         parameter.name = request->names[i];
         parameter.value = pvalue;
-        parameters.push_back(rclcpp::Parameter::from_parameter_msg(
-          parameter));
+        parameters.push_back(rclcpp::Parameter::from_parameter_msg(parameter));
       }
 
       promise_result->set_value(parameters);
@@ -211,10 +216,9 @@ AsyncParametersClient::set_parameters(
 
   auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
 
-  std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
-    [](rclcpp::Parameter p) {
-      return p.to_parameter_msg();
-    }
+  std::transform(
+    parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
+    [](rclcpp::Parameter p) {return p.to_parameter_msg();}
   );
 
   set_parameters_client_->async_send_request(
@@ -245,10 +249,9 @@ AsyncParametersClient::set_parameters_atomically(
 
   auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
 
-  std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
-    [](rclcpp::Parameter p) {
-      return p.to_parameter_msg();
-    }
+  std::transform(
+    parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
+    [](rclcpp::Parameter p) {return p.to_parameter_msg();}
   );
 
   set_parameters_atomically_client_->async_send_request(
@@ -411,8 +414,10 @@ SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_
 {
   auto f = async_parameters_client_->get_parameters(parameter_names);
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_base_interface_, f) ==
-    rclcpp::executor::FutureReturnCode::SUCCESS)
+  if (
+    spin_node_until_future_complete(
+      *executor_, node_base_interface_,
+      f) == rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -435,8 +440,10 @@ SyncParametersClient::get_parameter_types(const std::vector<std::string> & param
   auto f = async_parameters_client_->get_parameter_types(parameter_names);
 
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_base_interface_, f) ==
-    rclcpp::executor::FutureReturnCode::SUCCESS)
+  if (
+    spin_node_until_future_complete(
+      *executor_, node_base_interface_,
+      f) == rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -450,8 +457,10 @@ SyncParametersClient::set_parameters(
   auto f = async_parameters_client_->set_parameters(parameters);
 
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_base_interface_, f) ==
-    rclcpp::executor::FutureReturnCode::SUCCESS)
+  if (
+    spin_node_until_future_complete(
+      *executor_, node_base_interface_,
+      f) == rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -465,8 +474,10 @@ SyncParametersClient::set_parameters_atomically(
   auto f = async_parameters_client_->set_parameters_atomically(parameters);
 
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_base_interface_, f) ==
-    rclcpp::executor::FutureReturnCode::SUCCESS)
+  if (
+    spin_node_until_future_complete(
+      *executor_, node_base_interface_,
+      f) == rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }
@@ -482,8 +493,10 @@ SyncParametersClient::list_parameters(
   auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
 
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_base_interface_, f) ==
-    rclcpp::executor::FutureReturnCode::SUCCESS)
+  if (
+    spin_node_until_future_complete(
+      *executor_, node_base_interface_,
+      f) == rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
   }

rclcpp/src/rclcpp/parameter_service.cpp

@@ -59,10 +59,11 @@ ParameterService::ParameterService(
     {
       try {
         auto types = node_params->get_parameter_types(request->names);
-        std::transform(types.cbegin(), types.cend(),
-        std::back_inserter(response->types), [](const uint8_t & type) {
-          return static_cast<rclcpp::ParameterType>(type);
-        });
+        std::transform(
+          types.cbegin(), types.cend(),
+          std::back_inserter(response->types), [](const uint8_t & type) {
+            return static_cast<rclcpp::ParameterType>(type);
+          });
       } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) {
         RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what());
       }
@@ -103,11 +104,12 @@ ParameterService::ParameterService(
       std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Response> response)
     {
       std::vector<rclcpp::Parameter> pvariants;
-      std::transform(request->parameters.cbegin(), request->parameters.cend(),
-      std::back_inserter(pvariants),
-      [](const rcl_interfaces::msg::Parameter & p) {
-        return rclcpp::Parameter::from_parameter_msg(p);
-      });
+      std::transform(
+        request->parameters.cbegin(), request->parameters.cend(),
+        std::back_inserter(pvariants),
+        [](const rcl_interfaces::msg::Parameter & p) {
+          return rclcpp::Parameter::from_parameter_msg(p);
+        });
       try {
         auto result = node_params->set_parameters_atomically(pvariants);
         response->result = result;

rclcpp/src/rclcpp/publisher_base.cpp

@@ -32,7 +32,7 @@
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
-#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node.hpp"
 
@@ -84,14 +84,6 @@ PublisherBase::~PublisherBase()
   // must fini the events before fini-ing the publisher
   event_handlers_.clear();
 
-  if (rcl_publisher_fini(&intra_process_publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
-    RCUTILS_LOG_ERROR_NAMED(
-      "rclcpp",
-      "Error in destruction of intra process rcl publisher handle: %s",
-      rcl_get_error_string().str);
-    rcl_reset_error();
-  }
-
   if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
@@ -139,12 +131,6 @@ PublisherBase::get_gid() const
   return rmw_gid_;
 }
 
-const rmw_gid_t &
-PublisherBase::get_intra_process_gid() const
-{
-  return intra_process_rmw_gid_;
-}
-
 rcl_publisher_t *
 PublisherBase::get_publisher_handle()
 {
@@ -205,7 +191,7 @@ PublisherBase::get_intra_process_subscription_count() const
   return ipm->get_subscription_count(intra_process_publisher_id_);
 }
 
-rmw_qos_profile_t
+rclcpp::QoS
 PublisherBase::get_actual_qos() const
 {
   const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(&publisher_handle_);
@@ -214,7 +200,8 @@ PublisherBase::get_actual_qos() const
     rcl_reset_error();
     throw std::runtime_error(msg);
   }
-  return *qos;
+
+  return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
 }
 
 bool
@@ -223,6 +210,12 @@ PublisherBase::assert_liveliness() const
   return RCL_RET_OK == rcl_publisher_assert_liveliness(&publisher_handle_);
 }
 
+bool
+PublisherBase::can_loan_messages() const
+{
+  return rcl_publisher_can_loan_messages(&publisher_handle_);
+}
+
 bool
 PublisherBase::operator==(const rmw_gid_t & gid) const
 {
@@ -239,80 +232,15 @@ PublisherBase::operator==(const rmw_gid_t * gid) const
     rmw_reset_error();
     throw std::runtime_error(msg);
   }
-  if (!result) {
-    ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
-    if (ret != RMW_RET_OK) {
-      auto msg = std::string("failed to compare gids: ") + rmw_get_error_string().str;
-      rmw_reset_error();
-      throw std::runtime_error(msg);
-    }
-  }
   return result;
 }
 
-rclcpp::mapped_ring_buffer::MappedRingBufferBase::SharedPtr
-PublisherBase::make_mapped_ring_buffer(size_t size) const
-{
-  (void)size;
-  return nullptr;
-}
-
 void
 PublisherBase::setup_intra_process(
   uint64_t intra_process_publisher_id,
-  IntraProcessManagerSharedPtr ipm,
-  const rcl_publisher_options_t & intra_process_options)
+  IntraProcessManagerSharedPtr ipm)
 {
-  // Intraprocess configuration is not allowed with "durability" qos policy non "volatile".
-  if (this->get_actual_qos().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
-    throw std::invalid_argument(
-            "intraprocess communication is not allowed with durability qos policy non-volatile");
-  }
-  const char * topic_name = this->get_topic_name();
-  if (!topic_name) {
-    throw std::runtime_error("failed to get topic name");
-  }
-
-  auto intra_process_topic_name = std::string(topic_name) + "/_intra";
-
-  rcl_ret_t ret = rcl_publisher_init(
-    &intra_process_publisher_handle_,
-    rcl_node_handle_.get(),
-    rclcpp::type_support::get_intra_process_message_msg_type_support(),
-    intra_process_topic_name.c_str(),
-    &intra_process_options);
-  if (ret != RCL_RET_OK) {
-    if (ret == RCL_RET_TOPIC_NAME_INVALID) {
-      auto rcl_node_handle = rcl_node_handle_.get();
-      // this will throw on any validation problem
-      rcl_reset_error();
-      expand_topic_or_service_name(
-        intra_process_topic_name,
-        rcl_node_get_name(rcl_node_handle),
-        rcl_node_get_namespace(rcl_node_handle));
-    }
-
-    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process publisher");
-  }
-
   intra_process_publisher_id_ = intra_process_publisher_id;
   weak_ipm_ = ipm;
   intra_process_is_enabled_ = true;
-
-  // Life time of this object is tied to the publisher handle.
-  rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(
-    &intra_process_publisher_handle_);
-  if (publisher_rmw_handle == nullptr) {
-    auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string().str;
-    rcl_reset_error();
-    throw std::runtime_error(msg);
-  }
-  auto rmw_ret = rmw_get_gid_for_publisher(
-    publisher_rmw_handle, &intra_process_rmw_gid_);
-  if (rmw_ret != RMW_RET_OK) {
-    auto msg =
-      std::string("failed to create intra process publisher gid: ") + rmw_get_error_string().str;
-    rmw_reset_error();
-    throw std::runtime_error(msg);
-  }
 }

rclcpp/src/rclcpp/subscription_base.cpp

@@ -21,8 +21,9 @@
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
-#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/experimental/intra_process_manager.hpp"
 #include "rclcpp/logging.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
 
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
@@ -30,18 +31,19 @@
 using rclcpp::SubscriptionBase;
 
 SubscriptionBase::SubscriptionBase(
-  std::shared_ptr<rcl_node_t> node_handle,
+  rclcpp::node_interfaces::NodeBaseInterface * node_base,
   const rosidl_message_type_support_t & type_support_handle,
   const std::string & topic_name,
   const rcl_subscription_options_t & subscription_options,
   bool is_serialized)
-: node_handle_(node_handle),
+: node_base_(node_base),
+  node_handle_(node_base_->get_shared_rcl_node_handle()),
   use_intra_process_(false),
   intra_process_subscription_id_(0),
   type_support_(type_support_handle),
   is_serialized_(is_serialized)
 {
-  auto custom_deletor = [node_handle](rcl_subscription_t * rcl_subs)
+  auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs)
     {
       if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) {
         RCLCPP_ERROR(
@@ -57,10 +59,6 @@ SubscriptionBase::SubscriptionBase(
     new rcl_subscription_t, custom_deletor);
   *subscription_handle_.get() = rcl_get_zero_initialized_subscription();
 
-  intra_process_subscription_handle_ = std::shared_ptr<rcl_subscription_t>(
-    new rcl_subscription_t, custom_deletor);
-  *intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription();
-
   rcl_ret_t ret = rcl_subscription_init(
     subscription_handle_.get(),
     node_handle_.get(),
@@ -116,18 +114,25 @@ SubscriptionBase::get_subscription_handle() const
   return subscription_handle_;
 }
 
-const std::shared_ptr<rcl_subscription_t>
-SubscriptionBase::get_intra_process_subscription_handle() const
-{
-  return intra_process_subscription_handle_;
-}
-
 const std::vector<std::shared_ptr<rclcpp::QOSEventHandlerBase>> &
 SubscriptionBase::get_event_handlers() const
 {
   return event_handlers_;
 }
 
+rclcpp::QoS
+SubscriptionBase::get_actual_qos() const
+{
+  const rmw_qos_profile_t * qos = rcl_subscription_get_actual_qos(subscription_handle_.get());
+  if (!qos) {
+    auto msg = std::string("failed to get qos settings: ") + rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+
+  return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos);
+}
+
 const rosidl_message_type_support_t &
 SubscriptionBase::get_message_type_support_handle() const
 {
@@ -155,33 +160,52 @@ SubscriptionBase::get_publisher_count() const
   return inter_process_publisher_count;
 }
 
-void SubscriptionBase::setup_intra_process(
+void
+SubscriptionBase::setup_intra_process(
   uint64_t intra_process_subscription_id,
-  IntraProcessManagerWeakPtr weak_ipm,
-  const rcl_subscription_options_t & intra_process_options)
+  IntraProcessManagerWeakPtr weak_ipm)
 {
-  std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
-  rcl_ret_t ret = rcl_subscription_init(
-    intra_process_subscription_handle_.get(),
-    node_handle_.get(),
-    rclcpp::type_support::get_intra_process_message_msg_type_support(),
-    intra_process_topic_name.c_str(),
-    &intra_process_options);
-  if (ret != RCL_RET_OK) {
-    if (ret == RCL_RET_TOPIC_NAME_INVALID) {
-      auto rcl_node_handle = node_handle_.get();
-      // this will throw on any validation problem
-      rcl_reset_error();
-      expand_topic_or_service_name(
-        intra_process_topic_name,
-        rcl_node_get_name(rcl_node_handle),
-        rcl_node_get_namespace(rcl_node_handle));
-    }
-
-    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription");
-  }
-
   intra_process_subscription_id_ = intra_process_subscription_id;
   weak_ipm_ = weak_ipm;
   use_intra_process_ = true;
 }
+
+bool
+SubscriptionBase::can_loan_messages() const
+{
+  return rcl_subscription_can_loan_messages(subscription_handle_.get());
+}
+
+rclcpp::Waitable::SharedPtr
+SubscriptionBase::get_intra_process_waitable() const
+{
+  // If not using intra process, shortcut to nullptr.
+  if (!use_intra_process_) {
+    return nullptr;
+  }
+  // Get the intra process manager.
+  auto ipm = weak_ipm_.lock();
+  if (!ipm) {
+    throw std::runtime_error(
+            "SubscriptionBase::get_intra_process_waitable() called "
+            "after destruction of intra process manager");
+  }
+
+  // Use the id to retrieve the subscription intra-process from the intra-process manager.
+  return ipm->get_subscription_intra_process(intra_process_subscription_id_);
+}
+
+bool
+SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const
+{
+  if (!use_intra_process_) {
+    return false;
+  }
+  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);
+}

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -0,0 +1,38 @@
+// 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.
+
+#include "rclcpp/experimental/subscription_intra_process_base.hpp"
+
+using rclcpp::experimental::SubscriptionIntraProcessBase;
+
+bool
+SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+  rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL);
+  return RCL_RET_OK == ret;
+}
+
+const char *
+SubscriptionIntraProcessBase::get_topic_name() const
+{
+  return topic_name_.c_str();
+}
+
+rmw_qos_profile_t
+SubscriptionIntraProcessBase::get_actual_qos() const
+{
+  return qos_profile_;
+}

rclcpp/src/rclcpp/time.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <limits>
+#include <string>
 #include <utility>
 
 #include "rclcpp/clock.hpp"
@@ -194,7 +195,10 @@ Duration
 Time::operator-(const rclcpp::Time & rhs) const
 {
   if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
-    throw std::runtime_error("can't subtract times with different time sources");
+    throw std::runtime_error(
+            std::string("can't subtract times with different time sources [") +
+            std::to_string(rcl_time_.clock_type) + " != " +
+            std::to_string(rhs.rcl_time_.clock_type) + "]");
   }
 
   if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.rcl_time_.nanoseconds)) {

rclcpp/src/rclcpp/time_source.cpp

@@ -82,7 +82,7 @@ void TimeSource::attachNode(
   // Though this defaults to false, it can be overridden by initial parameter values for the node,
   // which may be given by the user at the node's construction or even by command-line arguments.
   rclcpp::ParameterValue use_sim_time_param;
-  const char * use_sim_time_name = "use_sim_time";
+  const std::string use_sim_time_name = "use_sim_time";
   if (!node_parameters_->has_parameter(use_sim_time_name)) {
     use_sim_time_param = node_parameters_->declare_parameter(
       use_sim_time_name,
@@ -98,11 +98,26 @@ void TimeSource::attachNode(
       create_clock_sub();
     }
   } else {
-    // TODO(wjwwood): use set_on_parameters_set_callback to catch the type mismatch,
-    //   before the use_sim_time parameter can ever be set to an invalid value
-    RCLCPP_ERROR(logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'",
+    RCLCPP_ERROR(
+      logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'",
       rclcpp::to_string(use_sim_time_param.get_type()).c_str());
   }
+  sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback(
+    [use_sim_time_name](const std::vector<rclcpp::Parameter> & parameters) {
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = true;
+      for (const auto & parameter : parameters) {
+        if (
+          parameter.get_name() == use_sim_time_name &&
+          parameter.get_type() != rclcpp::PARAMETER_BOOL)
+        {
+          result.successful = false;
+          result.reason = "'" + use_sim_time_name + "' must be a bool";
+          break;
+        }
+      }
+      return result;
+    });
 
   // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
   parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event(
@@ -121,6 +136,10 @@ void TimeSource::detachNode()
   node_services_.reset();
   node_logging_.reset();
   node_clock_.reset();
+  if (sim_time_cb_handler_ && node_parameters_) {
+    node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get());
+  }
+  sim_time_cb_handler_.reset();
   node_parameters_.reset();
   disable_ros_time();
 }
@@ -154,7 +173,8 @@ void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
 
 TimeSource::~TimeSource()
 {
-  if (node_base_ || node_topics_ || node_graph_ || node_services_ ||
+  if (
+    node_base_ || node_topics_ || node_graph_ || node_services_ ||
     node_logging_ || node_clock_ || node_parameters_)
   {
     this->detachNode();
@@ -220,13 +240,17 @@ void TimeSource::destroy_clock_sub()
 
 void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
 {
+  // Filter out events on 'use_sim_time' parameter instances in other nodes.
+  if (event->node != node_base_->get_fully_qualified_name()) {
+    return;
+  }
   // Filter for only 'use_sim_time' being added or changed.
   rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"},
     {rclcpp::ParameterEventsFilter::EventType::NEW,
       rclcpp::ParameterEventsFilter::EventType::CHANGED});
   for (auto & it : filter.get_events()) {
     if (it.second->value.type != ParameterType::PARAMETER_BOOL) {
-      RCLCPP_ERROR(logger_, "use_sim_time parameter set to something besides a bool");
+      RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool");
       continue;
     }
     if (it.second->value.bool_value) {

rclcpp/src/rclcpp/timer.cpp

@@ -17,6 +17,7 @@
 #include <chrono>
 #include <string>
 #include <memory>
+#include <thread>
 
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/exceptions.hpp"
@@ -40,11 +41,14 @@ TimerBase::TimerBase(
   timer_handle_ = std::shared_ptr<rcl_timer_t>(
     new rcl_timer_t, [ = ](rcl_timer_t * timer) mutable
     {
-      if (rcl_timer_fini(timer) != RCL_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str);
-        rcl_reset_error();
+      {
+        std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+        if (rcl_timer_fini(timer) != RCL_RET_OK) {
+          RCUTILS_LOG_ERROR_NAMED(
+            "rclcpp",
+            "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str);
+          rcl_reset_error();
+        }
       }
       delete timer;
       // Captured shared pointers by copy, reset to make sure timer is finalized before clock
@@ -54,15 +58,19 @@ TimerBase::TimerBase(
 
   *timer_handle_.get() = rcl_get_zero_initialized_timer();
 
-  rcl_clock_t * clock_handle = clock_->get_clock_handle();
-  if (rcl_timer_init(
-      timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr,
-      rcl_get_default_allocator()) != RCL_RET_OK)
   {
-    RCUTILS_LOG_ERROR_NAMED(
-      "rclcpp",
-      "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str);
-    rcl_reset_error();
+    std::lock_guard<std::mutex> clock_guard(clock_->get_clock_mutex());
+    rcl_clock_t * clock_handle = clock_->get_clock_handle();
+    if (
+      rcl_timer_init(
+        timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr,
+        rcl_get_default_allocator()) != RCL_RET_OK)
+    {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str);
+      rcl_reset_error();
+    }
   }
 }
 
@@ -110,12 +118,14 @@ std::chrono::nanoseconds
 TimerBase::time_until_trigger()
 {
   int64_t time_until_next_call = 0;
-  if (rcl_timer_get_time_until_next_call(timer_handle_.get(),
-    &time_until_next_call) != RCL_RET_OK)
+  if (
+    rcl_timer_get_time_until_next_call(
+      timer_handle_.get(),
+      &time_until_next_call) != RCL_RET_OK)
   {
     throw std::runtime_error(
-            std::string("Timer could not get time until next call: ") +
-            rcl_get_error_string().str);
+            std::string(
+              "Timer could not get time until next call: ") + rcl_get_error_string().str);
   }
   return std::chrono::nanoseconds(time_until_next_call);
 }

rclcpp/test/executors/test_multi_threaded_executor.cpp

@@ -89,10 +89,9 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) {
         double diff = std::abs((now - last).nanoseconds()) / 1.0e9;
         last = now;
 
-        if (diff < PERIOD - TOLERANCE || diff > PERIOD + TOLERANCE) {
+        if (diff < PERIOD - TOLERANCE) {
           executor.cancel();
           ASSERT_GT(diff, PERIOD - TOLERANCE);
-          ASSERT_LT(diff, PERIOD + TOLERANCE);
         }
       }
     };

rclcpp/test/mock_msgs/srv/Mock.srv

@@ -1,3 +0,0 @@
-bool request
----
-bool response

rclcpp/test/node_interfaces/test_get_node_interfaces.cpp

@@ -85,6 +85,26 @@ TEST_F(TestGetNodeInterfaces, node_reference) {
     >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *");
 }
 
+TEST_F(TestGetNodeInterfaces, rclcpp_node_pointer) {
+  rclcpp::Node * node_pointer = this->node.get();
+  auto result = rclcpp::node_interfaces::get_node_topics_interface(node_pointer);
+  static_assert(
+    std::is_same<
+      rclcpp::node_interfaces::NodeTopicsInterface *,
+      decltype(result)
+    >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *");
+}
+
+TEST_F(TestGetNodeInterfaces, node_pointer) {
+  NodeWrapper * wrapped_node_pointer = this->wrapped_node.get();
+  auto result = rclcpp::node_interfaces::get_node_topics_interface(wrapped_node_pointer);
+  static_assert(
+    std::is_same<
+      rclcpp::node_interfaces::NodeTopicsInterface *,
+      decltype(result)
+    >::value, "expected rclcpp::node_interfaces::NodeTopicsInterface *");
+}
+
 TEST_F(TestGetNodeInterfaces, interface_shared_pointer) {
   std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> interface_shared_ptr =
     this->node->get_node_topics_interface();

rclcpp/test/resources/test_node/test_parameters.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    parameter_int: 21
+    parameter_string_array: [baz, baz, baz]

rclcpp/test/test_client.cpp

@@ -75,12 +75,37 @@ TEST_F(TestClient, construction_and_destruction) {
   }
 
   {
-    ASSERT_THROW({
+    ASSERT_THROW(
+    {
       auto client = node->create_client<ListParameters>("invalid_service?");
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }
 
+TEST_F(TestClient, construction_with_free_function) {
+  {
+    auto client = rclcpp::create_client<rcl_interfaces::srv::ListParameters>(
+      node->get_node_base_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_services_interface(),
+      "service",
+      rmw_qos_profile_services_default,
+      nullptr);
+  }
+  {
+    ASSERT_THROW(
+    {
+      auto client = rclcpp::create_client<rcl_interfaces::srv::ListParameters>(
+        node->get_node_base_interface(),
+        node->get_node_graph_interface(),
+        node->get_node_services_interface(),
+        "invalid_?service",
+        rmw_qos_profile_services_default,
+        nullptr);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}
+
 /*
    Testing client construction and destruction for subnodes.
  */
@@ -92,7 +117,8 @@ TEST_F(TestClientSub, construction_and_destruction) {
   }
 
   {
-    ASSERT_THROW({
+    ASSERT_THROW(
+    {
       auto client = node->create_client<ListParameters>("invalid_service?");
     }, rclcpp::exceptions::InvalidServiceNameError);
   }

rclcpp/test/test_create_timer.cpp

@@ -0,0 +1,63 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include <atomic>
+#include <chrono>
+#include <memory>
+
+#include "rclcpp/create_timer.hpp"
+#include "rclcpp/executors.hpp"
+#include "rclcpp/node.hpp"
+#include "node_interfaces/node_wrapper.hpp"
+
+using namespace std::chrono_literals;
+
+TEST(TestCreateTimer, timer_executes)
+{
+  rclcpp::init(0, nullptr);
+  auto node = std::make_shared<rclcpp::Node>("test_create_timer_node");
+
+  std::atomic<bool> got_callback{false};
+
+  rclcpp::TimerBase::SharedPtr timer;
+  timer = rclcpp::create_timer(
+    node,
+    node->get_clock(),
+    rclcpp::Duration(0ms),
+    [&got_callback, &timer]() {
+      got_callback = true;
+      timer->cancel();
+    });
+
+  rclcpp::spin_some(node);
+
+  ASSERT_TRUE(got_callback);
+  rclcpp::shutdown();
+}
+
+TEST(TestCreateTimer, call_with_node_wrapper_compiles)
+{
+  rclcpp::init(0, nullptr);
+  NodeWrapper node("test_create_timer_call_with_node_wrapper_compiles");
+
+  rclcpp::TimerBase::SharedPtr timer;
+  timer = rclcpp::create_timer(
+    node,
+    node.get_node_clock_interface()->get_clock(),
+    rclcpp::Duration(0ms),
+    []() {});
+  rclcpp::shutdown();
+}

rclcpp/test/test_duration.cpp

@@ -136,3 +136,20 @@ TEST(TestDuration, maximum_duration) {
 
   EXPECT_EQ(max_duration, max);
 }
+
+static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000;
+static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS;
+
+TEST(TestDuration, from_seconds) {
+  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0.0));
+  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration::from_seconds(0));
+  EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(1.5));
+  EXPECT_EQ(rclcpp::Duration(-ONE_AND_HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(-1.5));
+}
+
+TEST(TestDuration, std_chrono_constructors) {
+  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0.0s));
+  EXPECT_EQ(rclcpp::Duration(0), rclcpp::Duration(0s));
+  EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration(1.5s));
+  EXPECT_EQ(rclcpp::Duration(-1, 0), rclcpp::Duration(-1s));
+}