0.7.1

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

CONTRIBUTING.md

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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,46 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.7.1 (2019-04-26)
+------------------
+* Added read only parameters. (`#495 <https://github.com/ros2/rclcpp/issues/495>`_)
+* Fixed a concurrency problem in the multithreaded executor. (`#703 <https://github.com/ros2/rclcpp/issues/703>`_)
+* Fixup utilities. (`#692 <https://github.com/ros2/rclcpp/issues/692>`_)
+* Added method to read timer cancellation. (`#697 <https://github.com/ros2/rclcpp/issues/697>`_)
+* Added Exception Generator function for implementing "from_rcl_error". (`#678 <https://github.com/ros2/rclcpp/issues/678>`_)
+* Updated initialization of rmw_qos_profile_t struct instances. (`#691 <https://github.com/ros2/rclcpp/issues/691>`_)
+* Removed the const value from the logger before comparison. (`#680 <https://github.com/ros2/rclcpp/issues/680>`_)
+* Contributors: Devin Bonnie, Dima Dorezyuk, Guillaume Autran, M. M, Shane Loretz, Víctor Mayoral Vilches, William Woodall, jhdcs
+
+0.7.0 (2019-04-14)
+------------------
+* Added Options-struct interfaces for creating publishers/subscribers (pre-QoS, standalone). (`#673 <https://github.com/ros2/rclcpp/issues/673>`_)
+* Replaced strncpy with memcpy. (`#684 <https://github.com/ros2/rclcpp/issues/684>`_)
+* Replaced const char * with a std::array<char, TOPIC_NAME_LENGTH> as the key of IPM IDTopicMap. (`#671 <https://github.com/ros2/rclcpp/issues/671>`_)
+* Refactored SignalHandler logger to avoid race during destruction. (`#682 <https://github.com/ros2/rclcpp/issues/682>`_)
+* Introduce rclcpp_components to implement composition. (`#665 <https://github.com/ros2/rclcpp/issues/665>`_)
+* Added QoS policy check when configuring intraprocess, skip interprocess publish when possible. (`#674 <https://github.com/ros2/rclcpp/issues/674>`_)
+* Updated to use do { .. } while(0) around content of logging macros. (`#681 <https://github.com/ros2/rclcpp/issues/681>`_)
+* Added function to get publisher's actual QoS settings. (`#667 <https://github.com/ros2/rclcpp/issues/667>`_)
+* Updated to avoid race that triggers timer too often. (`#621 <https://github.com/ros2/rclcpp/issues/621>`_)
+* Exposed get_fully_qualified_name in NodeBase API. (`#662 <https://github.com/ros2/rclcpp/issues/662>`_)
+* Updated to use ament_target_dependencies where possible. (`#659 <https://github.com/ros2/rclcpp/issues/659>`_)
+* Fixed wait for service memory leak bug. (`#656 <https://github.com/ros2/rclcpp/issues/656>`_)
+* Fixed test_time_source test. (`#639 <https://github.com/ros2/rclcpp/issues/639>`_)
+* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 <https://github.com/ros2/rclcpp/issues/648>`_)
+* Fixed cppcheck warning. (`#646 <https://github.com/ros2/rclcpp/issues/646>`_)
+* Added count matching api and intra-process subscriber count. (`#628 <https://github.com/ros2/rclcpp/issues/628>`_)
+* Added Sub Node alternative. (`#581 <https://github.com/ros2/rclcpp/issues/581>`_)
+* Replaced 'auto' with 'const auto &'. (`#630 <https://github.com/ros2/rclcpp/issues/630>`_)
+* Set Parameter Event Publisher settings. `#591 <https://github.com/ros2/rclcpp/issues/591>`_ (`#614 <https://github.com/ros2/rclcpp/issues/614>`_)
+* Replaced node constructor arguments with NodeOptions. (`#622 <https://github.com/ros2/rclcpp/issues/622>`_)
+* Updated to pass context to wait set (`#617 <https://github.com/ros2/rclcpp/issues/617>`_)
+* Added API to get parameters in a map. (`#575 <https://github.com/ros2/rclcpp/issues/575>`_)
+* Updated Bind usage since it is is no longer in std::__1. (`#618 <https://github.com/ros2/rclcpp/issues/618>`_)
+* Fixed errors from uncrustify v0.68. (`#613 <https://github.com/ros2/rclcpp/issues/613>`_)
+* Added new constructors for SyncParameterClient. (`#612 <https://github.com/ros2/rclcpp/issues/612>`_)
+* Contributors: Alberto Soragna, Chris Lalancette, Dirk Thomas, Emerson Knapp, Francisco Martín Rico, Jacob Perron, Marko Durkovic, Michael Carroll, Peter Baughman, Shane Loretz, Wei Liu, William Woodall, Yutaka Kondo, ivanpauno, kuzai, rarvolt
+
 0.6.2 (2018-12-13)
 ------------------
 * Updated to use signal safe synchronization with platform specific semaphores (`#607 <https://github.com/ros2/rclcpp/issues/607>`_)

rclcpp/CMakeLists.txt

@@ -47,6 +47,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/memory_strategies.cpp
   src/rclcpp/memory_strategy.cpp
   src/rclcpp/node.cpp
+  src/rclcpp/node_options.cpp
   src/rclcpp/node_interfaces/node_base.cpp
   src/rclcpp/node_interfaces/node_clock.cpp
   src/rclcpp/node_interfaces/node_graph.cpp
@@ -142,94 +143,90 @@ if(BUILD_TESTING)
 
   ament_add_gtest(test_client test/test_client.cpp)
   if(TARGET test_client)
-    target_include_directories(test_client PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_client
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_client ${PROJECT_NAME})
   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)
-    target_include_directories(test_expand_topic_or_service_name PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_expand_topic_or_service_name
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_function_traits test/test_function_traits.cpp)
   if(TARGET test_function_traits)
-    target_include_directories(test_function_traits PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_function_traits
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
   endif()
   ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp)
   if(TARGET test_mapped_ring_buffer)
-    target_include_directories(test_mapped_ring_buffer PUBLIC
-      ${rcl_INCLUDE_DIRS}
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    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)
   if(TARGET test_intra_process_manager)
-    target_include_directories(test_intra_process_manager PUBLIC
-      ${rcl_INCLUDE_DIRS}
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_intra_process_manager
+      "rcl"
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
   endif()
   ament_add_gtest(test_node test/test_node.cpp)
   if(TARGET test_node)
-    target_include_directories(test_node PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_node
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_node ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_node_global_args test/test_node_global_args.cpp)
   if(TARGET test_node_global_args)
-    target_include_directories(test_node_global_args PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_node_global_args
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_node_global_args ${PROJECT_NAME})
   endif()
-  ament_add_gtest(test_node_initial_parameters test/test_node_initial_parameters.cpp)
-  if(TARGET test_node_initial_parameters)
-    target_link_libraries(test_node_initial_parameters ${PROJECT_NAME})
-  endif()
   ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp)
   if(TARGET test_parameter_events_filter)
-    target_include_directories(test_parameter_events_filter PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_parameter_events_filter
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter_events_filter ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_parameter test/test_parameter.cpp)
   if(TARGET test_parameter)
-    target_include_directories(test_parameter PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_parameter
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_parameter ${PROJECT_NAME})
   endif()
@@ -239,21 +236,40 @@ if(BUILD_TESTING)
   endif()
   ament_add_gtest(test_publisher test/test_publisher.cpp)
   if(TARGET test_publisher)
-    target_include_directories(test_publisher PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_publisher
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_publisher ${PROJECT_NAME})
   endif()
+  ament_add_gtest(test_pub_sub_option_interface test/test_pub_sub_option_interface.cpp)
+  if(TARGET test_pub_sub_option_interface)
+    ament_target_dependencies(test_pub_sub_option_interface
+      test_msgs
+    )
+    target_link_libraries(test_pub_sub_option_interface
+      ${PROJECT_NAME}
+    )
+  endif()
+  ament_add_gtest(test_publisher_subscription_count_api test/test_publisher_subscription_count_api.cpp)
+  if(TARGET test_publisher_subscription_count_api)
+    ament_target_dependencies(test_publisher_subscription_count_api
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME})
+  endif()
   ament_add_gtest(test_rate test/test_rate.cpp)
   if(TARGET test_rate)
-    target_include_directories(test_rate PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_rate
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_rate
       ${PROJECT_NAME}
@@ -261,48 +277,55 @@ if(BUILD_TESTING)
   endif()
   ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp)
   if(TARGET test_serialized_message_allocator)
-    target_include_directories(test_serialized_message_allocator PUBLIC
-      ${test_msgs_INCLUDE_DIRS}
+    ament_target_dependencies(test_serialized_message_allocator
+      test_msgs
     )
     target_link_libraries(test_serialized_message_allocator
       ${PROJECT_NAME}
-      ${test_msgs_LIBRARIES}
     )
   endif()
   ament_add_gtest(test_service test/test_service.cpp)
   if(TARGET test_service)
-    target_include_directories(test_service PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_service
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_service ${PROJECT_NAME})
   endif()
   ament_add_gtest(test_subscription test/test_subscription.cpp)
   if(TARGET test_subscription)
-    target_include_directories(test_subscription PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_subscription
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_subscription ${PROJECT_NAME})
   endif()
+  ament_add_gtest(test_subscription_publisher_count_api test/test_subscription_publisher_count_api.cpp)
+  if(TARGET test_subscription_publisher_count_api)
+    ament_target_dependencies(test_subscription_publisher_count_api
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
+    )
+    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)
-    target_include_directories(test_subscription_traits PUBLIC
-      ${rcl_INCLUDE_DIRS}
+    ament_target_dependencies(test_subscription_traits
+      "rcl"
+      "test_msgs"
     )
-  ament_target_dependencies(test_subscription_traits
-    "test_msgs"
-  )
   endif()
   ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp)
   if(TARGET test_find_weak_nodes)
-    target_include_directories(test_find_weak_nodes PUBLIC
-      ${rcl_INCLUDE_DIRS}
+    ament_target_dependencies(test_find_weak_nodes
+      "rcl"
     )
     target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
   endif()
@@ -326,8 +349,8 @@ if(BUILD_TESTING)
   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)
-    target_include_directories(test_externally_defined_services PUBLIC
-      ${rcl_INCLUDE_DIRS}
+    ament_target_dependencies(test_externally_defined_services
+      "rcl"
     )
     target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
     foreach(typesupport_impl_cpp ${typesupport_impls_cpp})
@@ -370,6 +393,14 @@ if(BUILD_TESTING)
     target_link_libraries(test_time ${PROJECT_NAME})
   endif()
 
+  ament_add_gtest(test_timer test/test_timer.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_timer)
+    ament_target_dependencies(test_timer
+      "rcl")
+    target_link_libraries(test_timer ${PROJECT_NAME})
+  endif()
+
   ament_add_gtest(test_time_source test/test_time_source.cpp
     APPEND_LIBRARY_DIRS "${append_library_dirs}")
   if(TARGET test_time_source)
@@ -404,24 +435,17 @@ if(BUILD_TESTING)
 
   ament_add_gtest(test_local_parameters test/test_local_parameters.cpp)
   if(TARGET test_local_parameters)
-    target_include_directories(test_local_parameters PUBLIC
-      ${rcl_interfaces_INCLUDE_DIRS}
-      ${rmw_INCLUDE_DIRS}
-      ${rosidl_generator_cpp_INCLUDE_DIRS}
-      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    ament_target_dependencies(test_local_parameters
+      "rcl_interfaces"
+      "rmw"
+      "rosidl_generator_cpp"
+      "rosidl_typesupport_cpp"
     )
     target_link_libraries(test_local_parameters ${PROJECT_NAME})
   endif()
 endif()
 
-ament_package(
-  CONFIG_EXTRAS rclcpp-extras.cmake
-)
-
-install(
-  DIRECTORY cmake
-  DESTINATION share/${PROJECT_NAME}
-)
+ament_package()
 
 install(
   DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/

rclcpp/cmake/rclcpp_create_node_main.cmake

@@ -1,26 +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.
-
-
-set(rclcpp_node_main_SRC "${rclcpp_DIR}/../../../src/rclcpp/node_main.cpp")
-
-function(rclcpp_create_node_main node_library_target)
-  if(NOT TARGET ${node_library_target})
-    message(FATAL_ERROR "rclcpp_create_node_main() the first argument must be a valid target name")
-  endif()
-  set(executable_name_ ${node_library_target}_node)
-  add_executable(${executable_name_} ${rclcpp_node_main_SRC})
-  target_link_libraries(${executable_name_} ${node_library_target})
-  install(TARGETS ${executable_name_} DESTINATION bin)
-endfunction()

rclcpp/include/rclcpp/client.hpp

@@ -78,10 +78,10 @@ public:
   bool
   service_is_ready() const;
 
-  template<typename RatioT = std::milli>
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
-    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
     return wait_for_service_nanoseconds(
       std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)

rclcpp/include/rclcpp/exceptions.hpp

@@ -185,14 +185,35 @@ public:
 class InvalidParametersException : public std::runtime_error
 {
 public:
-  // Inherit constructors from runtime_error;
+  // Inherit constructors from runtime_error.
   using std::runtime_error::runtime_error;
 };
 
-/// Throwing if passed parameter value is invalid.
+/// Thrown if passed parameter value is invalid.
 class InvalidParameterValueException : public std::runtime_error
 {
-  // Inherit constructors from runtime_error;
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is already declared.
+class ParameterAlreadyDeclaredException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is not declared, e.g. either set or get was called without first declaring.
+class ParameterNotDeclaredException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
+/// Thrown if parameter is immutable and therefore cannot be undeclared.
+class ParameterImmutableException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
   using std::runtime_error::runtime_error;
 };
 

rclcpp/include/rclcpp/executor.hpp

@@ -151,11 +151,11 @@ public:
    * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this
    * function to be non-blocking.
    */
-  template<typename T = std::milli>
+  template<typename RepT = int64_t, typename T = std::milli>
   void
   spin_node_once(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node,
-    std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
+    std::chrono::duration<RepT, T> timeout = std::chrono::duration<RepT, T>(-1))
   {
     return spin_node_once_nanoseconds(
       node,
@@ -164,11 +164,11 @@ public:
   }
 
   /// Convenience function which takes Node and forwards NodeBaseInterface.
-  template<typename NodeT = rclcpp::Node, typename T = std::milli>
+  template<typename NodeT = rclcpp::Node, typename RepT = int64_t, typename T = std::milli>
   void
   spin_node_once(
     std::shared_ptr<NodeT> node,
-    std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
+    std::chrono::duration<RepT, T> timeout = std::chrono::duration<RepT, T>(-1))
   {
     return spin_node_once_nanoseconds(
       node->get_node_base_interface(),
@@ -218,11 +218,11 @@ public:
    *   code.
    * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
    */
-  template<typename ResponseT, typename TimeT = std::milli>
+  template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
   FutureReturnCode
   spin_until_future_complete(
     std::shared_future<ResponseT> & future,
-    std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+    std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
   {
     // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
     // inside a callback executed by an executor.

rclcpp/include/rclcpp/executors.hpp

@@ -65,13 +65,13 @@ using rclcpp::executors::SingleThreadedExecutor;
  *   If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code.
  * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
  */
-template<typename ResponseT, typename TimeT = std::milli>
+template<typename ResponseT, typename TimeRepT = int64_t, typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::executor::Executor & executor,
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
   std::shared_future<ResponseT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete
   // inside a callback executed by an executor.
@@ -81,13 +81,14 @@ spin_node_until_future_complete(
   return retcode;
 }
 
-template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeT = std::milli>
+template<typename NodeT = rclcpp::Node, typename ResponseT, typename TimeRepT = int64_t,
+  typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::executor::Executor & executor,
   std::shared_ptr<NodeT> node_ptr,
   std::shared_future<ResponseT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::executors::spin_node_until_future_complete(
     executor,
@@ -98,23 +99,24 @@ spin_node_until_future_complete(
 
 }  // namespace executors
 
-template<typename FutureT, typename TimeT = std::milli>
+template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_until_future_complete(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
   std::shared_future<FutureT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   rclcpp::executors::SingleThreadedExecutor executor;
   return executors::spin_node_until_future_complete<FutureT>(executor, node_ptr, future, timeout);
 }
 
-template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeT = std::milli>
+template<typename NodeT = rclcpp::Node, typename FutureT, typename TimeRepT = int64_t,
+  typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode
 spin_until_future_complete(
   std::shared_ptr<NodeT> node_ptr,
   std::shared_future<FutureT> & future,
-  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
 {
   return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout);
 }

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -78,7 +78,6 @@ private:
   size_t number_of_threads_;
   bool yield_before_execute_;
 
-  std::mutex scheduled_timers_mutex_;
   std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 

rclcpp/include/rclcpp/intra_process_manager.hpp

@@ -346,6 +346,11 @@ 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

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_
 
 #include <algorithm>
+#include <array>
 #include <atomic>
 #include <cstring>
 #include <functional>
@@ -24,10 +25,13 @@
 #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.hpp"
@@ -80,6 +84,9 @@ public:
   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)
 };
@@ -95,9 +102,7 @@ public:
   add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription)
   {
     subscriptions_[id] = subscription;
-    // subscription->get_topic_name() -> const char * can be used as the key,
-    // since subscriptions_ shares the ownership of subscription
-    subscription_ids_by_topic_[subscription->get_topic_name()].insert(id);
+    subscription_ids_by_topic_[fixed_size_string(subscription->get_topic_name())].insert(id);
   }
 
   void
@@ -172,7 +177,8 @@ public:
     }
 
     // Figure out what subscriptions should receive the message.
-    auto & destined_subscriptions = subscription_ids_by_topic_[publisher->get_topic_name()];
+    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(
@@ -248,9 +254,52 @@ public:
     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>;
 
@@ -262,19 +311,11 @@ private:
     std::hash<uint64_t>, std::equal_to<uint64_t>,
     RebindAlloc<std::pair<const uint64_t, SubscriptionBase::WeakPtr>>>;
 
-  struct strcmp_wrapper
-  {
-    bool
-    operator()(const char * lhs, const char * rhs) const
-    {
-      return std::strcmp(lhs, rhs) < 0;
-    }
-  };
   using IDTopicMap = std::map<
-    const char *,
+    FixedSizeString,
     AllocSet,
     strcmp_wrapper,
-    RebindAlloc<std::pair<const char * const, AllocSet>>>;
+    RebindAlloc<std::pair<const FixedSizeString, AllocSet>>>;
 
   SubscriptionMap subscriptions_;
 

rclcpp/include/rclcpp/intra_process_setting.hpp

@@ -0,0 +1,34 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__INTRA_PROCESS_SETTING_HPP_
+#define RCLCPP__INTRA_PROCESS_SETTING_HPP_
+
+namespace rclcpp
+{
+
+/// Used as argument in create_publisher and create_subscriber.
+enum class IntraProcessSetting
+{
+  /// Explicitly enable intraprocess comm at publisher/subscription level.
+  Enable,
+  /// Explicitly disable intraprocess comm at publisher/subscription level.
+  Disable,
+  /// Take intraprocess configuration from the node.
+  NodeDefault
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__INTRA_PROCESS_SETTING_HPP_

rclcpp/include/rclcpp/node_impl.hpp

@@ -51,34 +51,140 @@
 namespace rclcpp
 {
 
+RCLCPP_LOCAL
+inline
+std::string
+extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace)
+{
+  std::string name_with_sub_namespace(name);
+  if (sub_namespace != "" && name.front() != '/' && name.front() != '~') {
+    name_with_sub_namespace = sub_namespace + "/" + name;
+  }
+  return name_with_sub_namespace;
+}
+
+template<typename MessageT, typename AllocatorT, typename PublisherT>
+std::shared_ptr<PublisherT>
+Node::create_publisher(
+  const std::string & topic_name,
+  size_t qos_history_depth,
+  const PublisherOptionsWithAllocator<AllocatorT> & options)
+{
+  std::shared_ptr<AllocatorT> allocator = options.allocator;
+  if (!allocator) {
+    allocator = std::make_shared<AllocatorT>();
+  }
+  rmw_qos_profile_t qos_profile = options.qos_profile;
+  qos_profile.depth = qos_history_depth;
+
+  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 = this->get_node_options().use_intra_process_comms();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized IntraProcessSetting value");
+      break;
+  }
+
+  return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
+    this->node_topics_.get(),
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
+    qos_profile,
+    use_intra_process,
+    allocator);
+}
+
 template<typename MessageT, typename Alloc, typename PublisherT>
 std::shared_ptr<PublisherT>
 Node::create_publisher(
   const std::string & topic_name, size_t qos_history_depth,
-  std::shared_ptr<Alloc> allocator)
+  std::shared_ptr<Alloc> allocator,
+  IntraProcessSetting use_intra_process_comm)
 {
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
-  }
-  rmw_qos_profile_t qos = rmw_qos_profile_default;
-  qos.depth = qos_history_depth;
-  return this->create_publisher<MessageT, Alloc, PublisherT>(topic_name, qos, allocator);
+  PublisherOptionsWithAllocator<Alloc> pub_options;
+  pub_options.allocator = allocator;
+  pub_options.use_intra_process_comm = use_intra_process_comm;
+  return this->create_publisher<MessageT, Alloc, PublisherT>(
+    topic_name, qos_history_depth, pub_options);
 }
 
 template<typename MessageT, typename Alloc, typename PublisherT>
 std::shared_ptr<PublisherT>
 Node::create_publisher(
   const std::string & topic_name, const rmw_qos_profile_t & qos_profile,
-  std::shared_ptr<Alloc> allocator)
+  std::shared_ptr<Alloc> allocator, IntraProcessSetting use_intra_process_comm)
 {
+  PublisherOptionsWithAllocator<Alloc> pub_options;
+  pub_options.qos_profile = qos_profile;
+  pub_options.allocator = allocator;
+  pub_options.use_intra_process_comm = use_intra_process_comm;
+  return this->create_publisher<MessageT, Alloc, PublisherT>(
+    topic_name, qos_profile.depth, pub_options);
+}
+
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT,
+  typename SubscriptionT>
+std::shared_ptr<SubscriptionT>
+Node::create_subscription(
+  const std::string & topic_name,
+  CallbackT && callback,
+  size_t qos_history_depth,
+  const SubscriptionOptionsWithAllocator<AllocatorT> & options,
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, AllocatorT>::SharedPtr
+  msg_mem_strat)
+{
+  using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
+
+  std::shared_ptr<AllocatorT> allocator = options.allocator;
   if (!allocator) {
-    allocator = std::make_shared<Alloc>();
+    allocator = std::make_shared<AllocatorT>();
   }
-  return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
+
+  rmw_qos_profile_t qos_profile = options.qos_profile;
+  qos_profile.depth = qos_history_depth;
+
+  if (!msg_mem_strat) {
+    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
+    msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, AllocatorT>::create_default();
+  }
+
+  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 = this->get_node_options().use_intra_process_comms();
+      break;
+    default:
+      throw std::runtime_error("Unrecognized IntraProcessSetting value");
+      break;
+  }
+
+  return rclcpp::create_subscription<
+    MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>(
     this->node_topics_.get(),
-    topic_name,
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
+    std::forward<CallbackT>(callback),
     qos_profile,
-    use_intra_process_comms_,
+    options.callback_group,
+    options.ignore_local_publications,
+    use_intra_process,
+    msg_mem_strat,
     allocator);
 }
 
@@ -97,29 +203,18 @@ Node::create_subscription(
   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)
+  std::shared_ptr<Alloc> allocator,
+  IntraProcessSetting use_intra_process_comm)
 {
-  using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
+  SubscriptionOptionsWithAllocator<Alloc> sub_options;
+  sub_options.qos_profile = qos_profile;
+  sub_options.callback_group = group;
+  sub_options.ignore_local_publications = ignore_local_publications;
+  sub_options.allocator = allocator;
+  sub_options.use_intra_process_comm = use_intra_process_comm;
 
-  if (!allocator) {
-    allocator = std::make_shared<Alloc>();
-  }
-
-  if (!msg_mem_strat) {
-    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
-    msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, Alloc>::create_default();
-  }
-
-  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, CallbackMessageT, SubscriptionT>(
-    this->node_topics_.get(),
-    topic_name,
-    std::forward<CallbackT>(callback),
-    qos_profile,
-    group,
-    ignore_local_publications,
-    use_intra_process_comms_,
-    msg_mem_strat,
-    allocator);
+  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
+    topic_name, std::forward<CallbackT>(callback), qos_profile.depth, sub_options, msg_mem_strat);
 }
 
 template<
@@ -137,24 +232,23 @@ Node::create_subscription(
   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)
+  std::shared_ptr<Alloc> allocator,
+  IntraProcessSetting use_intra_process_comm)
 {
-  rmw_qos_profile_t qos = rmw_qos_profile_default;
-  qos.depth = qos_history_depth;
-  return this->create_subscription<MessageT>(
-    topic_name,
-    std::forward<CallbackT>(callback),
-    qos,
-    group,
-    ignore_local_publications,
-    msg_mem_strat,
-    allocator);
+  SubscriptionOptionsWithAllocator<Alloc> sub_options;
+  sub_options.callback_group = group;
+  sub_options.ignore_local_publications = ignore_local_publications;
+  sub_options.allocator = allocator;
+  sub_options.use_intra_process_comm = use_intra_process_comm;
+
+  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
+    topic_name, std::forward<CallbackT>(callback), qos_history_depth, sub_options, msg_mem_strat);
 }
 
-template<typename DurationT, typename CallbackT>
+template<typename DurationRepT, typename DurationT, typename CallbackT>
 typename rclcpp::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(
-  std::chrono::duration<int64_t, DurationT> period,
+  std::chrono::duration<DurationRepT, DurationT> period,
   CallbackT callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
@@ -182,7 +276,7 @@ Node::create_client(
   auto cli = Client<ServiceT>::make_shared(
     node_base_.get(),
     node_graph_,
-    service_name,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     options);
 
   auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
@@ -199,25 +293,68 @@ Node::create_service(
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
   return rclcpp::create_service<ServiceT, CallbackT>(
-    node_base_, node_services_,
-    service_name, std::forward<CallbackT>(callback), qos_profile, group);
-}
-
-template<typename CallbackT>
-void
-Node::register_param_change_callback(CallbackT && callback)
-{
-  this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
+    node_base_,
+    node_services_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    std::forward<CallbackT>(callback),
+    qos_profile,
+    group);
 }
 
 template<typename ParameterT>
-void
+auto
 Node::declare_parameter(
   const std::string & name,
   const ParameterT & default_value,
-  bool read_only)
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor)
 {
-  this->declare_parameter(name, rclcpp::ParameterValue(default_value), read_only);
+  return this->declare_parameter(
+    name,
+    rclcpp::ParameterValue(default_value),
+    parameter_descriptor
+  ).get<ParameterT>();
+}
+
+template<typename ParameterT>
+std::vector<ParameterT>
+Node::declare_parameters(
+  const std::string & namespace_,
+  const std::map<std::string, ParameterT> & parameters)
+{
+  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);
+    }
+  );
+  return result;
+}
+
+template<typename ParameterT>
+std::vector<ParameterT>
+Node::declare_parameters(
+  const std::string & namespace_,
+  const std::map<
+    std::string,
+    std::pair<ParameterT, rcl_interfaces::msg::ParameterDescriptor>
+  > & parameters)
+{
+  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 static_cast<ParameterT>(
+        this->declare_parameter(
+          normalized_namespace + element.first,
+          element.second.first,
+          element.second.second)
+      );
+    }
+  );
+  return result;
 }
 
 template<typename ParameterT>
@@ -226,30 +363,32 @@ Node::set_parameter_if_not_set(
   const std::string & name,
   const ParameterT & value)
 {
-  rclcpp::Parameter parameter;
-  if (!this->get_parameter(name, parameter)) {
-    this->set_parameters({
-        rclcpp::Parameter(name, 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 MapValueT>
+template<typename ParameterT>
 void
 Node::set_parameters_if_not_set(
   const std::string & name,
-  const std::map<std::string, MapValueT> & values)
+  const std::map<std::string, ParameterT> & values)
 {
   std::vector<rclcpp::Parameter> params;
 
   for (const auto & val : values) {
-    std::string param_name = name + "." + val.first;
-    rclcpp::Parameter parameter;
-    if (!this->get_parameter(param_name, parameter)) {
-      params.push_back(rclcpp::Parameter(param_name, val.second));
+    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));
     }
   }
 
@@ -258,32 +397,15 @@ Node::set_parameters_if_not_set(
 
 template<typename ParameterT>
 bool
-Node::get_parameter(const std::string & name, ParameterT & value) const
+Node::get_parameter(const std::string & name, ParameterT & parameter) const
 {
-  rclcpp::Parameter parameter;
-  bool result = get_parameter(name, parameter);
-  if (result) {
-    value = parameter.get_value<ParameterT>();
-  }
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
 
-  return result;
-}
+  rclcpp::Parameter parameter_variant;
 
-// this is a partially-specialized version of get_parameter above,
-// where our concrete type for ParameterT is std::map, but the to-be-determined
-// type is the value in the map.
-template<typename MapValueT>
-bool
-Node::get_parameters(
-  const std::string & name,
-  std::map<std::string, MapValueT> & values) const
-{
-  std::map<std::string, rclcpp::Parameter> params;
-  bool result = node_parameters_->get_parameters_by_prefix(name, params);
+  bool result = get_parameter(sub_name, parameter_variant);
   if (result) {
-    for (const auto & param : params) {
-      values[param.first] = param.second.get_value<MapValueT>();
-    }
+    parameter = static_cast<ParameterT>(parameter_variant.get_value<ParameterT>());
   }
 
   return result;
@@ -293,16 +415,38 @@ template<typename ParameterT>
 bool
 Node::get_parameter_or(
   const std::string & name,
-  ParameterT & value,
+  ParameterT & parameter,
   const ParameterT & alternative_value) const
 {
-  bool got_parameter = get_parameter(name, value);
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  bool got_parameter = get_parameter(sub_name, parameter);
   if (!got_parameter) {
-    value = alternative_value;
+    parameter = alternative_value;
   }
   return got_parameter;
 }
 
+// this is a partially-specialized version of get_parameter above,
+// where our concrete type for ParameterT is std::map, but the to-be-determined
+// type is the value in the map.
+template<typename ParameterT>
+bool
+Node::get_parameters(
+  const std::string & prefix,
+  std::map<std::string, ParameterT> & values) const
+{
+  std::map<std::string, rclcpp::Parameter> params;
+  bool result = node_parameters_->get_parameters_by_prefix(prefix, params);
+  if (result) {
+    for (const auto & param : params) {
+      values[param.first] = static_cast<ParameterT>(param.second.get_value<ParameterT>());
+    }
+  }
+
+  return result;
+}
+
 template<typename ParameterT>
 void
 Node::get_parameter_or_set(
@@ -310,15 +454,24 @@ Node::get_parameter_or_set(
   ParameterT & value,
   const ParameterT & alternative_value)
 {
-  bool got_parameter = get_parameter(name, 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(name, alternative_value),
+        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_parameters_interface.hpp

@@ -1,162 +0,0 @@
-// Copyright 2018 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__NODE_INTERFACES__GET_NODE_PARAMETERS_INTERFACE_HPP_
-#define RCLCPP__NODE_INTERFACES__GET_NODE_PARAMETERS_INTERFACE_HPP_
-
-#include <memory>
-#include <utility>
-#include <type_traits>
-
-#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
-
-/// This header provides the get_node_parameters_interface() template function.
-/**
- * This function is useful for getting the NodeParametersInterface pointer from
- * various kinds of Node-like classes.
- *
- * It's able to get the NodeParametersInterface pointer so long as the class
- * has a method called ``get_node_parameters_interface()`` which returns
- * either a pointer (const or not) to a NodeParametersInterface or a
- * std::shared_ptr to a NodeParametersInterface.
- */
-
-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_parameters_interface() which returns various types,
-// e.g. const pointer or a shared pointer.
-template<typename NodeType, typename ReturnType>
-struct has_get_node_parameters_interface
-{
-private:
-  template<typename T>
-  static constexpr
-  auto
-  check(T *)->typename std::is_same<
-    decltype(std::declval<T>().get_node_parameters_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 NodeParametersInterface already (just normal function overload).
-inline
-const rclcpp::node_interfaces::NodeParametersInterface *
-get_node_parameters_interface_from_pointer(
-  const rclcpp::node_interfaces::NodeParametersInterface * pointer)
-{
-  return pointer;
-}
-
-// If NodeType has a method called get_node_parameters_interface() which returns a shared pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_parameters_interface<
-    typename std::remove_pointer<NodeType>::type,
-    std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface>
-  >::value, int>::type = 0
->
-const rclcpp::node_interfaces::NodeParametersInterface *
-get_node_parameters_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_parameters_interface().get();
-}
-
-// If NodeType has a method called get_node_parameters_interface() which returns a pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_parameters_interface<
-    typename std::remove_pointer<NodeType>::type,
-    rclcpp::node_interfaces::NodeParametersInterface *
-  >::value, int>::type = 0
->
-const rclcpp::node_interfaces::NodeParametersInterface *
-get_node_parameters_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_parameters_interface();
-}
-
-// If NodeType has a method called get_node_parameters_interface() which returns a const pointer.
-template<
-  typename NodeType,
-  typename std::enable_if<has_get_node_parameters_interface<
-    typename std::remove_pointer<NodeType>::type,
-    const rclcpp::node_interfaces::NodeParametersInterface *
-  >::value, int>::type = 0
->
-const rclcpp::node_interfaces::NodeParametersInterface *
-get_node_parameters_interface_from_pointer(NodeType node_pointer)
-{
-  return node_pointer->get_node_parameters_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
->
-const rclcpp::node_interfaces::NodeParametersInterface *
-get_node_parameters_interface_from_pointer(NodeType node_shared_pointer)
-{
-  return get_node_parameters_interface_from_pointer(node_shared_pointer->get());
-}
-
-}  // namespace detail
-
-/// Get the NodeParametersInterface as a const pointer from a pointer to a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<std::is_pointer<NodeType>::value, int>::type = 0
->
-const rclcpp::node_interfaces::NodeParametersInterface *
-get_node_parameters_interface(NodeType && node_pointer)
-{
-  // Forward pointers to detail implmentation directly.
-  return detail::get_node_parameters_interface_from_pointer(std::forward<NodeType>(node_pointer));
-}
-
-/// Get the NodeParametersInterface as a const pointer from a "Node like" object.
-template<
-  typename NodeType,
-  typename std::enable_if<!std::is_pointer<NodeType>::value, int>::type = 0
->
-const rclcpp::node_interfaces::NodeParametersInterface *
-get_node_parameters_interface(NodeType && node_reference)
-{
-  // Forward const-references to detail implmentation as a pointer.
-  return detail::get_node_parameters_interface_from_pointer(&node_reference);
-}
-
-}  // namespace node_interfaces
-}  // namespace rclcpp
-
-#endif  // RCLCPP__NODE_INTERFACES__GET_NODE_PARAMETERS_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -21,6 +21,7 @@
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/node_options.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -39,9 +40,7 @@ public:
   NodeBase(
     const std::string & node_name,
     const std::string & namespace_,
-    rclcpp::Context::SharedPtr context,
-    const std::vector<std::string> & arguments,
-    bool use_global_arguments);
+    const NodeOptions & options);
 
   RCLCPP_PUBLIC
   virtual
@@ -57,6 +56,11 @@ public:
   const char *
   get_namespace() const;
 
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_fully_qualified_name() const;
+
   RCLCPP_PUBLIC
   virtual
   rclcpp::Context::SharedPtr

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -56,6 +56,13 @@ public:
   const char *
   get_namespace() const = 0;
 
+  /// Return the fully qualified name of the node.
+  /** \return The fully qualified name of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_fully_qualified_name() const = 0;
+
   /// Return the context of the node.
   /** \return SharedPtr to the node's context. */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -61,7 +61,6 @@ private:
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
-  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
 
   rclcpp::Clock::SharedPtr ros_clock_;

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -43,9 +43,6 @@ namespace node_interfaces
 // Internal struct for holding useful info about parameters
 struct ParameterInfo
 {
-  /// True if a user called declare_parameter()
-  bool is_declared = false;
-
   /// Current value of the parameter.
   rclcpp::ParameterValue value;
 
@@ -62,24 +59,36 @@ public:
   RCLCPP_PUBLIC
   NodeParameters(
     const node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     const node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
     const node_interfaces::NodeServicesInterface::SharedPtr node_services,
     const node_interfaces::NodeClockInterface::SharedPtr node_clock,
     const std::vector<Parameter> & initial_parameters,
     bool use_intra_process,
     bool start_parameter_services,
-    bool allow_undeclared_parameters);
+    bool start_parameter_event_publisher,
+    const rmw_qos_profile_t & parameter_event_qos_profile,
+    bool allow_undeclared_parameters,
+    bool automatically_declare_initial_parameters);
 
   RCLCPP_PUBLIC
   virtual
   ~NodeParameters();
 
   RCLCPP_PUBLIC
-  void
+  const rclcpp::ParameterValue &
   declare_parameter(
     const std::string & name,
     const rclcpp::ParameterValue & default_value,
-    bool read_only) override;
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor) override;
+
+  RCLCPP_PUBLIC
+  void
+  undeclare_parameter(const std::string & name) override;
+
+  RCLCPP_PUBLIC
+  bool
+  has_parameter(const std::string & name) const override;
 
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
@@ -123,16 +132,25 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
+  RCLCPP_PUBLIC
+  OnParametersSetCallbackType
+  set_on_parameters_set_callback(OnParametersSetCallbackType callback) override;
+
+  [[deprecated("use set_on_parameters_set_callback() instead")]]
   RCLCPP_PUBLIC
   void
-  register_param_change_callback(ParametersCallbackFunction callback) override;
+  register_param_change_callback(OnParametersSetCallbackType callback) override;
+
+  RCLCPP_PUBLIC
+  const std::map<std::string, rclcpp::ParameterValue> &
+  get_initial_parameter_values() const override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
 
   mutable std::mutex mutex_;
 
-  ParametersCallbackFunction parameters_callback_ = nullptr;
+  OnParametersSetCallbackType on_parameters_set_callback_ = nullptr;
 
   std::map<std::string, ParameterInfo> parameters_;
 
@@ -146,6 +164,7 @@ private:
 
   std::string combined_name_;
 
+  node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
   node_interfaces::NodeClockInterface::SharedPtr node_clock_;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -38,35 +38,53 @@ class NodeParametersInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface)
 
-  /// Declare and initialize a parameter.
-  /* This method is used to declare that a parameter exists on this node.
-   * If a run-time user has provided an an initial value then it will be set in this method,
-   * otherwise the default_value will be set.
-   * \param[in] name the name of the parameter
-   * \param[in] default_value An initial value to be used if a run-time user did not override it.
-   * \param[in] read_only if True then this parameter may not be changed after initialization.
-   * \throws std::runtime_error if parameter has already been declared.
-   * \throws std::runtime_error if a parameter name is invalid.
-   * \throws rclcpp::exceptions::InvalidParameterValueException if initial value fails to be set.
-   */
-  RCLCPP_PUBLIC
-  virtual
-  void
-  declare_parameter(
-    const std::string & name,
-    const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
-    bool read_only = false) = 0;
-
   RCLCPP_PUBLIC
   virtual
   ~NodeParametersInterface() = default;
 
+  /// Declare and initialize a parameter.
+  /**
+   * \sa rclcpp::Node::declare_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  const rclcpp::ParameterValue &
+  declare_parameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value,
+    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor) = 0;
+
+  /// Undeclare a parameter.
+  /**
+   * \sa rclcpp::Node::undeclare_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  undeclare_parameter(const std::string & name) = 0;
+
+  /// Return true if the parameter has been declared, otherwise false.
+  /**
+   * \sa rclcpp::Node::has_parameter
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  has_parameter(const std::string & name) const = 0;
+
+  /// Set one or more parameters, one at a time.
+  /**
+   * \sa rclcpp::Node::set_parameters
+   */
   RCLCPP_PUBLIC
   virtual
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(
-    const std::vector<rclcpp::Parameter> & parameters) = 0;
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;
 
+  /// Set and initialize a parameter, all at once.
+  /**
+   * \sa rclcpp::Node::set_parameters_atomically
+   */
   RCLCPP_PUBLIC
   virtual
   rcl_interfaces::msg::SetParametersResult
@@ -138,14 +156,34 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
 
-  using ParametersCallbackFunction = std::function<
-    rcl_interfaces::msg::SetParametersResult(
-      const std::vector<rclcpp::Parameter> &)>;
+  using OnParametersSetCallbackType =
+    std::function<
+    rcl_interfaces::msg::SetParametersResult(const std::vector<rclcpp::Parameter> &)
+    >;
 
+  using ParametersCallbackFunction [[deprecated("use OnParametersSetCallbackType instead")]] =
+    OnParametersSetCallbackType;
+
+  /// Register a callback for when parameters are being set, return an existing one.
+  /**
+   * \sa rclcpp::Node::set_on_parameters_set_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  OnParametersSetCallbackType
+  set_on_parameters_set_callback(OnParametersSetCallbackType callback) = 0;
+
+  [[deprecated("use set_on_parameters_set_callback() instead")]]
   RCLCPP_PUBLIC
   virtual
   void
-  register_param_change_callback(ParametersCallbackFunction callback) = 0;
+  register_param_change_callback(OnParametersSetCallbackType callback) = 0;
+
+  /// Return the initial parameter values used by the NodeParameters to override default values.
+  RCLCPP_PUBLIC
+  virtual
+  const std::map<std::string, rclcpp::ParameterValue> &
+  get_initial_parameter_values() const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_options.hpp

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

rclcpp/include/rclcpp/parameter.hpp

@@ -51,21 +51,37 @@ get_value_helper(const rclcpp::Parameter * parameter);
 class Parameter
 {
 public:
+  /// Construct with an empty name and a parameter value of type rclcpp::PARAMETER_NOT_SET.
   RCLCPP_PUBLIC
   Parameter();
 
+  /// Construct with given name and a parameter value of type rclcpp::PARAMETER_NOT_SET.
+  RCLCPP_PUBLIC
+  explicit Parameter(const std::string & name);
+
+  /// Construct with given name and given parameter value.
   RCLCPP_PUBLIC
   Parameter(const std::string & name, const ParameterValue & value);
 
+  /// Construct with given name and given parameter value.
   template<typename ValueTypeT>
-  explicit Parameter(const std::string & name, ValueTypeT value)
+  Parameter(const std::string & name, ValueTypeT value)
   : Parameter(name, ParameterValue(value))
-  {
-  }
+  {}
 
   RCLCPP_PUBLIC
   explicit Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info);
 
+  /// Equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator==(const Parameter & rhs) const;
+
+  /// Not equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator!=(const Parameter & rhs) const;
+
   RCLCPP_PUBLIC
   ParameterType
   get_type() const;
@@ -191,7 +207,7 @@ inline
 auto
 get_value_helper<rclcpp::Parameter>(const rclcpp::Parameter * parameter)
 {
-  // Use this labmda to ensure it's a const reference being returned (and not a copy).
+  // Use this lambda to ensure it's a const reference being returned (and not a copy).
   auto type_enforcing_lambda =
     [&parameter]() -> const rclcpp::Parameter & {
       return *parameter;

rclcpp/include/rclcpp/parameter_client.hpp

@@ -138,10 +138,10 @@ public:
   bool
   service_is_ready() const;
 
-  template<typename RatioT = std::milli>
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
-    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
     return wait_for_service_nanoseconds(
       std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
@@ -281,10 +281,10 @@ public:
     return async_parameters_client_->service_is_ready();
   }
 
-  template<typename RatioT = std::milli>
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_service(
-    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
     return async_parameters_client_->wait_for_service(timeout);
   }

rclcpp/include/rclcpp/parameter_service.hpp

@@ -43,7 +43,7 @@ public:
   explicit ParameterService(
     const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
     const std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
-    node_interfaces::NodeParametersInterface * node_params,
+    rclcpp::node_interfaces::NodeParametersInterface * node_params,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
 private:

rclcpp/include/rclcpp/parameter_value.hpp

@@ -130,10 +130,21 @@ public:
   rcl_interfaces::msg::ParameterValue
   to_value_msg() const;
 
+  /// Equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator==(const ParameterValue & rhs) const;
+
+  /// Not equal operator.
+  RCLCPP_PUBLIC
+  bool
+  operator!=(const ParameterValue & rhs) const;
+
   // The following get() variants require the use of ParameterType
 
   template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, bool>::type
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, const bool &>::type
   get() const
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
@@ -143,7 +154,8 @@ public:
   }
 
   template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, const int64_t &>::type
   get() const
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
@@ -153,7 +165,8 @@ public:
   }
 
   template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, double>::type
+  constexpr
+  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, const double &>::type
   get() const
   {
     if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
@@ -163,6 +176,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<type == ParameterType::PARAMETER_STRING, const std::string &>::type
   get() const
   {
@@ -173,6 +187,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<
     type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector<uint8_t> &>::type
   get() const
@@ -184,6 +199,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<
     type == ParameterType::PARAMETER_BOOL_ARRAY, const std::vector<bool> &>::type
   get() const
@@ -195,6 +211,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<
     type == ParameterType::PARAMETER_INTEGER_ARRAY, const std::vector<int64_t> &>::type
   get() const
@@ -206,6 +223,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<
     type == ParameterType::PARAMETER_DOUBLE_ARRAY, const std::vector<double> &>::type
   get() const
@@ -217,6 +235,7 @@ public:
   }
 
   template<ParameterType type>
+  constexpr
   typename std::enable_if<
     type == ParameterType::PARAMETER_STRING_ARRAY, const std::vector<std::string> &>::type
   get() const
@@ -230,28 +249,32 @@ public:
   // The following get() variants allow the use of primitive types
 
   template<typename type>
-  typename std::enable_if<std::is_same<type, bool>::value, bool>::type
+  constexpr
+  typename std::enable_if<std::is_same<type, bool>::value, const bool &>::type
   get() const
   {
     return get<ParameterType::PARAMETER_BOOL>();
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
-    std::is_integral<type>::value && !std::is_same<type, bool>::value, int64_t>::type
+    std::is_integral<type>::value && !std::is_same<type, bool>::value, const int64_t &>::type
   get() const
   {
     return get<ParameterType::PARAMETER_INTEGER>();
   }
 
   template<typename type>
-  typename std::enable_if<std::is_floating_point<type>::value, double>::type
+  constexpr
+  typename std::enable_if<std::is_floating_point<type>::value, const double &>::type
   get() const
   {
     return get<ParameterType::PARAMETER_DOUBLE>();
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<std::is_convertible<type, std::string>::value, const std::string &>::type
   get() const
   {
@@ -259,6 +282,7 @@ public:
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
     std::is_convertible<
       type, const std::vector<uint8_t> &>::value, const std::vector<uint8_t> &>::type
@@ -268,6 +292,7 @@ public:
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
     std::is_convertible<
       type, const std::vector<bool> &>::value, const std::vector<bool> &>::type
@@ -277,6 +302,7 @@ public:
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
     std::is_convertible<
       type, const std::vector<int64_t> &>::value, const std::vector<int64_t> &>::type
@@ -286,6 +312,7 @@ public:
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
     std::is_convertible<
       type, const std::vector<double> &>::value, const std::vector<double> &>::type
@@ -295,6 +322,7 @@ public:
   }
 
   template<typename type>
+  constexpr
   typename std::enable_if<
     std::is_convertible<
       type, const std::vector<std::string> &>::value, const std::vector<std::string> &>::type

rclcpp/include/rclcpp/publisher.hpp

@@ -45,6 +45,15 @@ namespace node_interfaces
 class NodeTopicsInterface;
 }
 
+namespace intra_process_manager
+{
+/**
+ * NOTE(ivanpauno): IntraProcessManager is forward declared here, avoiding a circular inclusion between intra_process_manager.hpp and publisher.hpp.
+ * SharedPtr and WeakPtr of the IntraProcessManager are defined again here, to avoid a warning for accessing a member of a forward declared class.
+ */
+class IntraProcessManager;
+}
+
 class PublisherBase
 {
   friend ::rclcpp::node_interfaces::NodeTopicsInterface;
@@ -107,6 +116,32 @@ public:
   const rcl_publisher_t *
   get_publisher_handle() const;
 
+  /// Get subscription count
+  /** \return The number of subscriptions. */
+  RCLCPP_PUBLIC
+  size_t
+  get_subscription_count() const;
+
+  /// Get intraprocess subscription count
+  /** \return The number of intraprocess subscriptions. */
+  RCLCPP_PUBLIC
+  size_t
+  get_intra_process_subscription_count() 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
+  rmw_qos_profile_t
+  get_actual_qos() const;
+
   /// Compare this publisher to a gid.
   /**
    * Note that this function calls the next function.
@@ -128,13 +163,16 @@ public:
   operator==(const rmw_gid_t * gid) const;
 
   using StoreMessageCallbackT = std::function<uint64_t(uint64_t, void *, const std::type_info &)>;
+  using IntraProcessManagerSharedPtr =
+    std::shared_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
 
   /// Implementation utility function used to setup intra process publishing after creation.
   RCLCPP_PUBLIC
   void
   setup_intra_process(
     uint64_t intra_process_publisher_id,
-    StoreMessageCallbackT callback,
+    StoreMessageCallbackT store_callback,
+    IntraProcessManagerSharedPtr ipm,
     const rcl_publisher_options_t & intra_process_options);
 
 protected:
@@ -143,6 +181,10 @@ protected:
   rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher();
   rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher();
 
+  using IntraProcessManagerWeakPtr =
+    std::weak_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
+  bool intra_process_is_enabled_;
+  IntraProcessManagerWeakPtr weak_ipm_;
   uint64_t intra_process_publisher_id_;
   StoreMessageCallbackT store_intra_process_message_;
 
@@ -188,7 +230,11 @@ public:
   virtual void
   publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
   {
-    this->do_inter_process_publish(msg.get());
+    bool inter_process_subscriptions_exist =
+      get_subscription_count() > get_intra_process_subscription_count();
+    if (!intra_process_is_enabled_ || inter_process_subscriptions_exist) {
+      this->do_inter_process_publish(msg.get());
+    }
     if (store_intra_process_message_) {
       // Take the pointer from the unique_msg, release it and pass as a void *
       // to the ipm. The ipm should then capture it again as a unique_ptr of

rclcpp/include/rclcpp/publisher_options.hpp

@@ -0,0 +1,44 @@
+// 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__PUBLISHER_OPTIONS_HPP_
+#define RCLCPP__PUBLISHER_OPTIONS_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/intra_process_setting.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Structure containing optional configuration for Publishers.
+template<typename Allocator>
+struct PublisherOptionsWithAllocator
+{
+  /// The quality of service profile to pass on to the rmw implementation.
+  rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
+  /// Optional custom allocator.
+  std::shared_ptr<Allocator> allocator = nullptr;
+  /// Setting to explicitly set intraprocess communications.
+  IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault;
+};
+
+using PublisherOptions = PublisherOptionsWithAllocator<std::allocator<void>>;
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PUBLISHER_OPTIONS_HPP_

rclcpp/include/rclcpp/subscription.hpp

@@ -46,6 +46,15 @@ namespace node_interfaces
 class NodeTopicsInterface;
 }  // namespace node_interfaces
 
+namespace intra_process_manager
+{
+/**
+ * NOTE(ivanpauno): IntraProcessManager is forward declared here, avoiding a circular inclusion between intra_process_manager.hpp and publisher.hpp.
+ * SharedPtr and WeakPtr of the IntraProcessManager are defined again here, to avoid a warning for accessing a member of a forward declared class.
+ */
+class IntraProcessManager;
+}
+
 /// Virtual base class for subscriptions. This pattern allows us to iterate over different template
 /// specializations of Subscription, among other things.
 class SubscriptionBase
@@ -129,11 +138,23 @@ public:
   bool
   is_serialized() const;
 
+  /// Get matching publisher count
+  /** \return The number of publishers on this topic. */
+  RCLCPP_PUBLIC
+  size_t
+  get_publisher_count() const;
+
 protected:
   std::shared_ptr<rcl_subscription_t> intra_process_subscription_handle_;
   std::shared_ptr<rcl_subscription_t> subscription_handle_;
   std::shared_ptr<rcl_node_t> node_handle_;
 
+  using IntraProcessManagerWeakPtr =
+    std::weak_ptr<rclcpp::intra_process_manager::IntraProcessManager>;
+  bool use_intra_process_;
+  IntraProcessManagerWeakPtr weak_ipm_;
+  uint64_t intra_process_subscription_id_;
+
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
 
@@ -272,10 +293,13 @@ public:
   using MatchesAnyPublishersCallbackType = std::function<bool (const rmw_gid_t *)>;
 
   /// Implemenation detail.
+  // TODO(ivanpauno): This can be moved to the base class. No reason to be here.
+  // Also get_intra_process_message_callback_ and matches_any_intra_process_publishers_.
   void setup_intra_process(
     uint64_t intra_process_subscription_id,
     GetMessageCallbackType get_message_callback,
     MatchesAnyPublishersCallbackType matches_any_publisher_callback,
+    IntraProcessManagerWeakPtr weak_ipm,
     const rcl_subscription_options_t & intra_process_options)
   {
     std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
@@ -302,6 +326,8 @@ public:
     intra_process_subscription_id_ = intra_process_subscription_id;
     get_intra_process_message_callback_ = get_message_callback;
     matches_any_intra_process_publishers_ = matches_any_publisher_callback;
+    weak_ipm_ = weak_ipm;
+    use_intra_process_ = true;
   }
 
   /// Implemenation detail.
@@ -323,7 +349,6 @@ private:
 
   GetMessageCallbackType get_intra_process_message_callback_;
   MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_;
-  uint64_t intra_process_subscription_id_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -166,6 +166,7 @@ create_subscription_factory(
         intra_process_subscription_id,
         take_intra_process_message_func,
         matches_any_publisher_func,
+        weak_ipm,
         intra_process_options
       );
     };

rclcpp/include/rclcpp/subscription_options.hpp

@@ -0,0 +1,48 @@
+// 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__SUBSCRIPTION_OPTIONS_HPP_
+#define RCLCPP__SUBSCRIPTION_OPTIONS_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/intra_process_setting.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Structure containing optional configuration for Subscriptions.
+template<typename Allocator>
+struct SubscriptionOptionsWithAllocator
+{
+  /// The quality of service profile to pass on to the rmw implementation.
+  rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
+  /// 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;
+  /// Optional custom allocator.
+  std::shared_ptr<Allocator> allocator = nullptr;
+  /// Setting to explicitly set intraprocess communications.
+  IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault;
+};
+
+using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_OPTIONS_HPP_

rclcpp/include/rclcpp/timer.hpp

@@ -58,6 +58,16 @@ public:
   void
   cancel();
 
+  /// Return the timer cancellation state.
+  /**
+   * \return true if the timer has been cancelled, false otherwise
+   * \throws std::runtime_error if the rcl_get_error_state returns 0
+   * \throws RCLErrorBase some child class exception based on ret
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_canceled();
+
   RCLCPP_PUBLIC
   void
   reset();

rclcpp/include/rclcpp/unused_parameters_checker.hpp

@@ -1,115 +0,0 @@
-// Copyright 2018 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP__UNUSED_PARAMETERS_CHECKER_HPP_
-#define RCLCPP__UNUSED_PARAMETERS_CHECKER_HPP_
-
-#include <memory>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp"
-#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
-#include "rclcpp/macros.hpp"
-#include "rclcpp/parameter.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-namespace rclcpp
-{
-
-/// Check a Node-like class for unused parameters.
-/**
- * This class can be used to detect misconfigurations and typos by ensuring all
- * initial parameter values that were passed to the Node-like object were used.
- * So this class's methods are used after or during Node construction and after
- * all parameters have been declared.
- *
- * This class must not outlive the Node that it's being used with.
- */
-class UnusedParametersChecker
-{
-public:
-  template<typename NodeType>
-  explicit UnusedParametersChecker(NodeType && node_like)
-  : node_parameters_interface_(
-      rclcpp::node_interfaces::get_node_parameters_interface(std::forward<NodeType>(node_like)))
-  {}
-
-  /// Warn if any initial parameter values have not been used.
-  /**
-   * This function will complain with a RCLCPP_WARN if any provided initial
-   * parameter values have not been used.
-   *
-   * \throws std::bad_alloc when trying to create an error message
-   */
-  RCLCPP_PUBLIC
-  void
-  check_and_warn() const;
-
-  /// Throw an UnusedParameterExecption if any initial parameter values have not been used.
-  /**
-   * This function will throw an exception if any provided initial
-   * parameter values have not been used.
-   *
-   * \throws std::bad_alloc when trying to create an error message
-   * \throws rclcpp::UnusedParametersException when there are unused parameters
-   */
-  RCLCPP_PUBLIC
-  void
-  check_and_throw() const;
-
-  /// Return the number of unused initial parameter values.
-  /**
-   * Similar to get_unused_initial_parameter_values(), but it returns the
-   * number of unused parameter values rather than a vector of the unused
-   * parameters (which involves allocating storage for the copies).
-   * This function is faster and avoids memory allocation while checking for a
-   * problem, and if one is detected then get_unused_initial_parameter_values()
-   * may be used to format a useful error message.
-   *
-   * \returns the number of unused initial parameter values
-   */
-  RCLCPP_PUBLIC
-  size_t
-  count_unused_initial_parameter_values() const;
-
-  /// Return the list of unused initial parameter values.
-  /**
-   * A common case where this returns a non-empty vector, is when someone makes
-   * a typo when setting the parameters from outside the node.
-   * For example, if they use `ip_addr` rather than the expected `ip_address`.
-   *
-   * \returns vector of parameters which where passed to the node but where
-   *   not declared before this function was called.
-   * \throws std::bad_alloc
-   */
-  RCLCPP_PUBLIC
-  std::vector<rclcpp::Parameter>
-  get_unused_initial_parameter_values() const;
-
-private:
-  const rclcpp::node_interfaces::NodeParametersInterface * node_parameters_interface_;
-};
-
-/// Thrown when throw_if_unused_initialized_parameter_values() finds unused parameters.
-class UnusedParametersException : public std::runtime_error
-{
-public:
-  explicit UnusedParametersException(const std::vector<rclcpp::Parameter> & unused_parameters);
-};
-
-}  // namespace rclcpp
-
-#endif  // RCLCPP__UNUSED_PARAMETERS_CHECKER_HPP_

rclcpp/include/rclcpp/utilities.hpp

@@ -18,17 +18,14 @@
 #include <chrono>
 #include <functional>
 #include <limits>
+#include <string>
 #include <vector>
 
 #include "rclcpp/context.hpp"
 #include "rclcpp/init_options.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-#include "rmw/macros.h"
-#include "rmw/rmw.h"
-
 #ifdef ANDROID
-#include <string>
 #include <sstream>
 
 namespace std
@@ -104,6 +101,7 @@ uninstall_signal_handlers();
  *
  * \sa rclcpp::Context::init() for more details on arguments and possible exceptions
  * \returns Members of the argument vector that are not ROS arguments.
+ * \throws anything remove_ros_arguments can throw
  */
 RCLCPP_PUBLIC
 std::vector<std::string>
@@ -122,6 +120,8 @@ init_and_remove_ros_arguments(
  * \param[in] argc Number of arguments.
  * \param[in] argv Argument vector.
  * \returns Members of the argument vector that are not ROS arguments.
+ * \throws anything throw_from_rcl_error can throw
+ * \throws rclcpp::exceptions::RCLErrorBase if the parsing fails
  */
 RCLCPP_PUBLIC
 std::vector<std::string>

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.6.2</version>
+  <version>0.7.1</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>

rclcpp/resource/logging.hpp.em

@@ -71,6 +71,9 @@ def is_supported_feature_combination(feature_combination):
 #else
 @[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@
 @{suffix = get_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)@
@@ -90,18 +93,20 @@ def is_supported_feature_combination(feature_combination):
  * It also accepts a single argument of type std::string.
  */
 #define RCLCPP_@(severity)@(suffix)(logger, @(''.join([p + ', ' for p in get_macro_parameters(feature_combination).keys()]))...) \
-  static_assert( \
-    ::std::is_same<typename std::remove_reference<decltype(logger)>::type, \
-    typename ::rclcpp::Logger>::value, \
-    "First argument to logging macros must be an rclcpp::Logger"); \
-  RCUTILS_LOG_@(severity)@(suffix)_NAMED( \
+  do { \
+    static_assert( \
+      ::std::is_same<typename std::remove_reference<typename std::remove_cv<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 params]@
-@(''.join(['    ' + p + ', \\\n' for p in params]))@
+@(''.join(['      ' + p + ', \\\n' for p in params]))@
 @[ end if]@
-    logger.get_name(), \
-    rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \
-      RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,""))
+      logger.get_name(), \
+      rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \
+        RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,"")); \
+  } while (0)
 
 @[ end for]@
 #endif

rclcpp/src/rclcpp/client.cpp

@@ -118,7 +118,6 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
   if (!node_ptr) {
     throw InvalidNodeError();
   }
-  auto event = node_ptr->get_graph_event();
   // check to see if the server is ready immediately
   if (this->service_is_ready()) {
     return true;
@@ -127,6 +126,7 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
     // check was non-blocking, return immediately
     return false;
   }
+  auto event = node_ptr->get_graph_event();
   // update the time even on the first loop to account for time spent in the first call
   // to this->server_is_ready()
   std::chrono::nanoseconds time_to_wait =

rclcpp/src/rclcpp/clock.cpp

@@ -130,10 +130,15 @@ Clock::create_jump_callback(
       rclcpp::Clock::on_time_jump, handler);
   if (RCL_RET_OK != ret) {
     delete handler;
-    handler = NULL;
+    handler = nullptr;
     rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
   }
 
+  if (nullptr == handler) {
+    // imposible to reach here; added to make cppcheck happy
+    return nullptr;
+  }
+
   // *INDENT-OFF*
   // create shared_ptr that removes the callback automatically when all copies are destructed
   return rclcpp::JumpHandler::SharedPtr(handler, [this](rclcpp::JumpHandler * handler) noexcept {

rclcpp/src/rclcpp/exceptions.cpp

@@ -39,8 +39,8 @@ NameValidationError::format_error(
   return msg;
 }
 
-void
-throw_from_rcl_error(
+std::exception_ptr
+from_rcl_error(
   rcl_ret_t ret,
   const std::string & prefix,
   const rcl_error_state_t * error_state,
@@ -55,9 +55,9 @@ throw_from_rcl_error(
   if (!error_state) {
     throw std::runtime_error("rcl error state is not set");
   }
-  std::string formated_prefix = prefix;
+  std::string formatted_prefix = prefix;
   if (!prefix.empty()) {
-    formated_prefix += ": ";
+    formatted_prefix += ": ";
   }
   RCLErrorBase base_exc(ret, error_state);
   if (reset_error) {
@@ -65,14 +65,28 @@ throw_from_rcl_error(
   }
   switch (ret) {
     case RCL_RET_BAD_ALLOC:
-      throw RCLBadAlloc(base_exc);
+      return std::make_exception_ptr(RCLBadAlloc(base_exc));
     case RCL_RET_INVALID_ARGUMENT:
-      throw RCLInvalidArgument(base_exc, formated_prefix);
+      return std::make_exception_ptr(RCLInvalidArgument(base_exc, formatted_prefix));
     default:
-      throw RCLError(base_exc, formated_prefix);
+      return std::make_exception_ptr(RCLError(base_exc, formatted_prefix));
   }
 }
 
+void
+throw_from_rcl_error(
+  rcl_ret_t ret,
+  const std::string & prefix,
+  const rcl_error_state_t * error_state,
+  void (* reset_error)())
+{
+  // We expect this to either throw a standard error,
+  // or to generate an error pointer (which is caught
+  // in err, and immediately thrown)
+  auto err = from_rcl_error(ret, prefix, error_state, reset_error);
+  std::rethrow_exception(err);
+}
+
 RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state)
 : ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number),
   formatted_message(rcl_get_error_string().str)

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -82,7 +82,6 @@ MultiThreadedExecutor::run(size_t)
       }
       if (any_exec.timer) {
         // Guard against multiple threads getting the same timer.
-        std::lock_guard<std::mutex> lock(scheduled_timers_mutex_);
         if (scheduled_timers_.count(any_exec.timer) != 0) {
           continue;
         }
@@ -96,11 +95,14 @@ MultiThreadedExecutor::run(size_t)
     execute_any_executable(any_exec);
 
     if (any_exec.timer) {
-      std::lock_guard<std::mutex> lock(scheduled_timers_mutex_);
+      std::lock_guard<std::mutex> wait_lock(wait_mutex_);
       auto it = scheduled_timers_.find(any_exec.timer);
       if (it != scheduled_timers_.end()) {
         scheduled_timers_.erase(it);
       }
     }
+    // Clear the callback_group to prevent the AnyExecutable destructor from
+    // resetting the callback group `can_be_taken_from`
+    any_exec.callback_group.reset();
   }
 }

rclcpp/src/rclcpp/intra_process_manager.cpp

@@ -56,6 +56,12 @@ IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const
   return impl_->matches_any_publishers(id);
 }
 
+size_t
+IntraProcessManager::get_subscription_count(uint64_t intra_process_publisher_id) const
+{
+  return impl_->get_subscription_count(intra_process_publisher_id);
+}
+
 uint64_t
 IntraProcessManager::get_next_unique_id()
 {

rclcpp/src/rclcpp/node.cpp

@@ -27,43 +27,89 @@
 #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"
 #include "rclcpp/node_interfaces/node_topics.hpp"
 #include "rclcpp/node_interfaces/node_waitables.hpp"
 
+#include "rmw/validate_namespace.h"
+
 using rclcpp::Node;
+using rclcpp::NodeOptions;
 using rclcpp::exceptions::throw_from_rcl_error;
 
+RCLCPP_LOCAL
+std::string
+extend_sub_namespace(const std::string & existing_sub_namespace, const std::string & extension)
+{
+  // Assumption is that the existing_sub_namespace does not need checking
+  // because it would be checked already when it was set with this function.
+
+  // check if the new sub-namespace extension is absolute
+  if (extension.front() == '/') {
+    throw rclcpp::exceptions::NameValidationError(
+            "sub_namespace",
+            extension.c_str(),
+            "a sub-namespace should not have a leading /",
+            0);
+  }
+
+  std::string new_sub_namespace;
+  if (existing_sub_namespace.empty()) {
+    new_sub_namespace = extension;
+  } else {
+    new_sub_namespace = existing_sub_namespace + "/" + extension;
+  }
+
+  // remove any trailing `/` so that new extensions do no result in `//`
+  if (new_sub_namespace.back() == '/') {
+    new_sub_namespace = new_sub_namespace.substr(0, new_sub_namespace.size() - 1);
+  }
+
+  return new_sub_namespace;
+}
+
+RCLCPP_LOCAL
+std::string
+create_effective_namespace(const std::string & node_namespace, const std::string & sub_namespace)
+{
+  // Assumption is that both the node_namespace and sub_namespace are conforming
+  // and do not need trimming of `/` and other things, as they were validated
+  // in other functions already.
+
+  if (node_namespace.back() == '/') {
+    // this is the special case where node_namespace is just `/`
+    return node_namespace + sub_namespace;
+  } else {
+    return node_namespace + "/" + sub_namespace;
+  }
+}
+
 Node::Node(
   const std::string & node_name,
-  const std::string & namespace_,
-  bool use_intra_process_comms)
-: Node(
-    node_name,
-    namespace_,
-    rclcpp::contexts::default_context::get_global_default_context(),
-    {},
-    {},
-    true,
-    use_intra_process_comms,
-    true)
-{}
+  const NodeOptions & options)
+: Node(node_name, "", options)
+{
+}
 
 Node::Node(
   const std::string & node_name,
   const std::string & namespace_,
-  rclcpp::Context::SharedPtr context,
-  const std::vector<std::string> & arguments,
-  const std::vector<rclcpp::Parameter> & initial_parameters,
-  bool use_global_arguments,
-  bool use_intra_process_comms,
-  bool start_parameter_services,
-  bool allow_undeclared_parameters)
+  const NodeOptions & options)
 : node_base_(new rclcpp::node_interfaces::NodeBase(
-      node_name, namespace_, context, arguments, use_global_arguments)),
+      node_name, namespace_, options)),
   node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
   node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())),
   node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
@@ -78,13 +124,17 @@ Node::Node(
     )),
   node_parameters_(new rclcpp::node_interfaces::NodeParameters(
       node_base_,
+      node_logging_,
       node_topics_,
       node_services_,
       node_clock_,
-      initial_parameters,
-      use_intra_process_comms,
-      start_parameter_services,
-      allow_undeclared_parameters
+      options.initial_parameters(),
+      options.use_intra_process_comms(),
+      options.start_parameter_services(),
+      options.start_parameter_event_publisher(),
+      options.parameter_event_qos_profile(),
+      options.allow_undeclared_parameters(),
+      options.automatically_declare_initial_parameters()
     )),
   node_time_source_(new rclcpp::node_interfaces::NodeTimeSource(
       node_base_,
@@ -96,10 +146,48 @@ Node::Node(
       node_parameters_
     )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
-  use_intra_process_comms_(use_intra_process_comms)
+  node_options_(options),
+  sub_namespace_(""),
+  effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_))
 {
 }
 
+Node::Node(
+  const Node & other,
+  const std::string & sub_namespace)
+: node_base_(other.node_base_),
+  node_graph_(other.node_graph_),
+  node_logging_(other.node_logging_),
+  node_timers_(other.node_timers_),
+  node_topics_(other.node_topics_),
+  node_services_(other.node_services_),
+  node_clock_(other.node_clock_),
+  node_parameters_(other.node_parameters_),
+  node_options_(other.node_options_),
+  sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)),
+  effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_))
+{
+  // Validate new effective namespace.
+  int validation_result;
+  size_t invalid_index;
+  rmw_ret_t rmw_ret =
+    rmw_validate_namespace(effective_namespace_.c_str(), &validation_result, &invalid_index);
+
+  if (rmw_ret != RMW_RET_OK) {
+    if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+      throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate subnode namespace");
+    }
+    throw_from_rcl_error(RCL_RET_ERROR, "failed to validate subnode namespace");
+  }
+
+  if (validation_result != RMW_NAMESPACE_VALID) {
+    throw rclcpp::exceptions::InvalidNamespaceError(
+            effective_namespace_.c_str(),
+            rmw_namespace_validation_result_string(validation_result),
+            invalid_index);
+  }
+}
+
 Node::~Node()
 {}
 
@@ -115,6 +203,12 @@ Node::get_namespace() const
   return node_base_->get_namespace();
 }
 
+const char *
+Node::get_fully_qualified_name() const
+{
+  return node_base_->get_fully_qualified_name();
+}
+
 rclcpp::Logger
 Node::get_logger() const
 {
@@ -134,29 +228,57 @@ Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group)
   return node_base_->callback_group_in_node(group);
 }
 
-void
+const rclcpp::ParameterValue &
 Node::declare_parameter(
   const std::string & name,
   const rclcpp::ParameterValue & default_value,
-  bool read_only)
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor)
 {
-  return this->node_parameters_->declare_parameter(name, default_value, read_only);
+  return this->node_parameters_->declare_parameter(name, default_value, parameter_descriptor);
+}
+
+void
+Node::undeclare_parameter(const std::string & name)
+{
+  this->node_parameters_->undeclare_parameter(name);
+}
+
+bool
+Node::has_parameter(const std::string & name) const
+{
+  return this->node_parameters_->has_parameter(name);
+}
+
+rcl_interfaces::msg::SetParametersResult
+Node::set_parameter(const rclcpp::Parameter & parameter)
+{
+  return this->set_parameters_atomically({parameter});
 }
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
-Node::set_parameters(
-  const std::vector<rclcpp::Parameter> & parameters)
+Node::set_parameters(const std::vector<rclcpp::Parameter> & parameters)
 {
   return node_parameters_->set_parameters(parameters);
 }
 
 rcl_interfaces::msg::SetParametersResult
-Node::set_parameters_atomically(
-  const std::vector<rclcpp::Parameter> & parameters)
+Node::set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters)
 {
   return node_parameters_->set_parameters_atomically(parameters);
 }
 
+rclcpp::Parameter
+Node::get_parameter(const std::string & name) const
+{
+  return node_parameters_->get_parameter(name);
+}
+
+bool
+Node::get_parameter(const std::string & name, rclcpp::Parameter & parameter) const
+{
+  return node_parameters_->get_parameter(name, parameter);
+}
+
 std::vector<rclcpp::Parameter>
 Node::get_parameters(
   const std::vector<std::string> & names) const
@@ -164,40 +286,43 @@ Node::get_parameters(
   return node_parameters_->get_parameters(names);
 }
 
-rclcpp::Parameter
-Node::get_parameter(const std::string & name) const
+rcl_interfaces::msg::ParameterDescriptor
+Node::describe_parameter(const std::string & name) const
 {
-  return node_parameters_->get_parameter(name);
-}
-
-bool Node::get_parameter(
-  const std::string & name,
-  rclcpp::Parameter & parameter) const
-{
-  return node_parameters_->get_parameter(name, parameter);
+  auto result = node_parameters_->describe_parameters({name});
+  if (0 == result.size()) {
+    throw rclcpp::exceptions::ParameterNotDeclaredException(name);
+  }
+  if (result.size() > 1) {
+    throw std::runtime_error("number of described parameters unexpectedly more than one");
+  }
+  return result.front();
 }
 
 std::vector<rcl_interfaces::msg::ParameterDescriptor>
-Node::describe_parameters(
-  const std::vector<std::string> & names) const
+Node::describe_parameters(const std::vector<std::string> & names) const
 {
   return node_parameters_->describe_parameters(names);
 }
 
 std::vector<uint8_t>
-Node::get_parameter_types(
-  const std::vector<std::string> & names) const
+Node::get_parameter_types(const std::vector<std::string> & names) const
 {
   return node_parameters_->get_parameter_types(names);
 }
 
 rcl_interfaces::msg::ListParametersResult
-Node::list_parameters(
-  const std::vector<std::string> & prefixes, uint64_t depth) const
+Node::list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const
 {
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
+rclcpp::Node::OnParametersSetCallbackType
+Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback)
+{
+  return node_parameters_->set_on_parameters_set_callback(callback);
+}
+
 std::vector<std::string>
 Node::get_node_names() const
 {
@@ -319,3 +444,29 @@ Node::get_node_waitables_interface()
 {
   return node_waitables_;
 }
+
+const std::string &
+Node::get_sub_namespace() const
+{
+  return this->sub_namespace_;
+}
+
+const std::string &
+Node::get_effective_namespace() const
+{
+  return this->effective_namespace_;
+}
+
+Node::SharedPtr
+Node::create_sub_node(const std::string & sub_namespace)
+{
+  // Cannot use make_shared<Node>() here as it requires the constructor to be
+  // public, and this constructor is intentionally protected instead.
+  return std::shared_ptr<Node>(new Node(*this, sub_namespace));
+}
+
+const NodeOptions &
+Node::get_node_options() const
+{
+  return this->node_options_;
+}

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -32,10 +32,8 @@ using rclcpp::node_interfaces::NodeBase;
 NodeBase::NodeBase(
   const std::string & node_name,
   const std::string & namespace_,
-  rclcpp::Context::SharedPtr context,
-  const std::vector<std::string> & arguments,
-  bool use_global_arguments)
-: context_(context),
+  const rclcpp::NodeOptions & options)
+: context_(options.context()),
   node_handle_(nullptr),
   default_callback_group_(nullptr),
   associated_with_executor_(false),
@@ -44,7 +42,7 @@ NodeBase::NodeBase(
   // Setup the guard condition that is notified when changes occur in the graph.
   rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
   rcl_ret_t ret = rcl_guard_condition_init(
-    &notify_guard_condition_, context->get_rcl_context().get(), guard_condition_options);
+    &notify_guard_condition_, options.context()->get_rcl_context().get(), guard_condition_options);
   if (ret != RCL_RET_OK) {
     throw_from_rcl_error(ret, "failed to create interrupt guard condition");
   }
@@ -59,76 +57,16 @@ NodeBase::NodeBase(
       }
     };
 
-  // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable.
-  size_t domain_id = 0;
-  char * ros_domain_id = nullptr;
-  const char * env_var = "ROS_DOMAIN_ID";
-#ifndef _WIN32
-  ros_domain_id = getenv(env_var);
-#else
-  size_t ros_domain_id_size;
-  _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
-#endif
-  if (ros_domain_id) {
-    uint32_t number = strtoul(ros_domain_id, NULL, 0);
-    if (number == (std::numeric_limits<uint32_t>::max)()) {
-      // Finalize the interrupt guard condition.
-      finalize_notify_guard_condition();
-#ifdef _WIN32
-      // free the ros_domain_id before throwing, if getenv was used on Windows
-      free(ros_domain_id);
-#endif
-
-      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
-    }
-    domain_id = static_cast<size_t>(number);
-#ifdef _WIN32
-    free(ros_domain_id);
-#endif
-  }
-
   // Create the rcl node and store it in a shared_ptr with a custom destructor.
   std::unique_ptr<rcl_node_t> rcl_node(new rcl_node_t(rcl_get_zero_initialized_node()));
 
-  rcl_node_options_t options = rcl_node_get_default_options();
-  std::unique_ptr<const char *[]> c_args;
-  if (!arguments.empty()) {
-    c_args.reset(new const char *[arguments.size()]);
-    for (std::size_t i = 0; i < arguments.size(); ++i) {
-      c_args[i] = arguments[i].c_str();
-    }
-  }
-  // TODO(sloretz) Pass an allocator to argument parsing
-  if (arguments.size() > std::numeric_limits<int>::max()) {
-    throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args");
-  }
-  ret = rcl_parse_arguments(
-    static_cast<int>(arguments.size()), c_args.get(), rcl_get_default_allocator(),
-    &(options.arguments));
-  if (RCL_RET_OK != ret) {
-    finalize_notify_guard_condition();
-    throw_from_rcl_error(ret, "failed to parse arguments");
-  }
-
-  options.use_global_arguments = use_global_arguments;
-  // TODO(wjwwood): pass the Allocator to the options
-  options.domain_id = domain_id;
-
   ret = rcl_node_init(
     rcl_node.get(),
     node_name.c_str(), namespace_.c_str(),
-    context->get_rcl_context().get(), &options);
+    options.context()->get_rcl_context().get(), options.get_rcl_node_options());
   if (ret != RCL_RET_OK) {
     // Finalize the interrupt guard condition.
     finalize_notify_guard_condition();
-    // Finalize previously allocated node arguments
-    if (RCL_RET_OK != rcl_arguments_fini(&options.arguments)) {
-      // Print message because exception will be thrown later in this code block
-      RCUTILS_LOG_ERROR_NAMED(
-        "rclcpp",
-        "Failed to fini arguments during error handling: %s", rcl_get_error_string().str);
-      rcl_reset_error();
-    }
 
     if (ret == RCL_RET_NODE_INVALID_NAME) {
       rcl_reset_error();  // discard rcl_node_init error
@@ -195,15 +133,6 @@ NodeBase::NodeBase(
 
   // Indicate the notify_guard_condition is now valid.
   notify_guard_condition_is_valid_ = true;
-
-  // Finalize previously allocated node arguments
-  if (RCL_RET_OK != rcl_arguments_fini(&options.arguments)) {
-    // print message because throwing would prevent the destructor from being called
-    RCUTILS_LOG_ERROR_NAMED(
-      "rclcpp",
-      "Failed to fini arguments: %s", rcl_get_error_string().str);
-    rcl_reset_error();
-  }
 }
 
 NodeBase::~NodeBase()
@@ -232,6 +161,12 @@ NodeBase::get_namespace() const
   return rcl_node_get_namespace(node_handle_.get());
 }
 
+const char *
+NodeBase::get_fully_qualified_name() const
+{
+  return rcl_node_get_fully_qualified_name(node_handle_.get());
+}
+
 rclcpp::Context::SharedPtr
 NodeBase::get_context()
 {

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -12,7 +12,17 @@
 // 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>
 
@@ -34,14 +44,21 @@ using rclcpp::node_interfaces::NodeParameters;
 
 NodeParameters::NodeParameters(
   const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
   const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
   const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
   const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
   const std::vector<rclcpp::Parameter> & initial_parameters,
   bool use_intra_process,
   bool start_parameter_services,
-  bool allow_undeclared_parameters)
-: allow_undeclared_(allow_undeclared_parameters), node_clock_(node_clock)
+  bool start_parameter_event_publisher,
+  const rmw_qos_profile_t & parameter_event_qos_profile,
+  bool allow_undeclared_parameters,
+  bool automatically_declare_initial_parameters)
+: allow_undeclared_(allow_undeclared_parameters),
+  events_publisher_(nullptr),
+  node_logging_(node_logging),
+  node_clock_(node_clock)
 {
   using MessageT = rcl_interfaces::msg::ParameterEvent;
   using PublisherT = rclcpp::Publisher<MessageT>;
@@ -53,12 +70,14 @@ NodeParameters::NodeParameters(
     parameter_service_ = std::make_shared<ParameterService>(node_base, node_services, this);
   }
 
-  events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
-    node_topics.get(),
-    "parameter_events",
-    rmw_qos_profile_parameter_events,
-    use_intra_process,
-    allocator);
+  if (start_parameter_event_publisher) {
+    events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
+      node_topics.get(),
+      "parameter_events",
+      parameter_event_qos_profile,
+      use_intra_process,
+      allocator);
+  }
 
   // Get the node options
   const rcl_node_t * node = node_base->get_rcl_node_handle();
@@ -150,10 +169,16 @@ NodeParameters::NodeParameters(
       rclcpp::ParameterValue(param.get_value_message());
   }
 
-  if (!initial_parameters.empty() && allow_undeclared_) {
-    rcl_interfaces::msg::SetParametersResult result = set_parameters_atomically(initial_parameters);
-    if (!result.successful) {
-      throw std::runtime_error("Failed to set initial parameters");
+  // If asked, initialize any parameters that ended up in the initial parameter values,
+  // but did not get declared explcitily by this point.
+  if (automatically_declare_initial_parameters) {
+    for (const auto & pair : this->get_initial_parameter_values()) {
+      if (!this->has_parameter(pair.first)) {
+        this->declare_parameter(
+          pair.first,
+          pair.second,
+          rcl_interfaces::msg::ParameterDescriptor());
+      }
     }
   }
 }
@@ -161,171 +186,352 @@ NodeParameters::NodeParameters(
 NodeParameters::~NodeParameters()
 {}
 
-void
-NodeParameters::declare_parameter(
-  const std::string & name,
-  const rclcpp::ParameterValue & default_value,
-  bool read_only)
+RCLCPP_LOCAL
+bool
+__lockless_has_parameter(
+  const std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameters,
+  const std::string & name)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
-  // TODO(sloretz) parameter name validation
-  if (name.empty()) {
-    throw std::runtime_error("parameter name must not be empty");
+  return parameters.find(name) != parameters.end();
+}
+
+using OnParametersSetCallbackType =
+  rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
+
+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)
+{
+  // 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);
   }
 
-  // Error if this parameter has already been declared and is different
-  auto param_iter = parameters_.find(name);
-  if (param_iter != parameters_.end() && param_iter->second.is_declared) {
-    if (
-      param_iter->second.descriptor.type != default_value.get_type() ||
-      param_iter->second.descriptor.read_only != read_only)
-    {
-      throw std::runtime_error("parameter '" + name + "' exists with conflicting description");
+  // If accepted, actually set the values.
+  if (result.successful) {
+    for (size_t i = 0; i < parameters.size(); ++i) {
+      const std::string & name = parameters[i].get_name();
+      parameter_infos[name].descriptor.name = parameters[i].get_name();
+      parameter_infos[name].descriptor.type = parameters[i].get_type();
+      parameter_infos[name].value = parameters[i].get_parameter_value();
     }
   }
 
-  // Check if run-time user passed an initial value, else use the default.
-  rclcpp::ParameterValue initial_value = default_value;
-  auto value_iter = initial_parameter_values_.find(name);
-  if (value_iter != initial_parameter_values_.end()) {
-    initial_value = value_iter->second;
+  // Either way, return the result.
+  return result;
+}
+
+RCLCPP_LOCAL
+rcl_interfaces::msg::SetParametersResult
+__declare_parameter_common(
+  const std::string & name,
+  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)
+{
+  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.
+  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;
   }
 
-  // Save a description of the parameter
-  rcl_interfaces::msg::ParameterDescriptor desc;
-  desc.name = name;
-  desc.type = initial_value.get_type();
-  desc.read_only = read_only;
+  // Check with the user's callback to see if the initial value can be set.
+  std::vector<rclcpp::Parameter> parameter_wrappers {rclcpp::Parameter(name, *initial_value)};
+  // This function also takes care of default vs initial value.
+  auto result = __set_parameters_atomically_common(
+    parameter_wrappers,
+    parameter_infos,
+    on_set_parameters_callback);
 
-  rclcpp::node_interfaces::ParameterInfo pinfo;
-  pinfo.is_declared = true;
-  pinfo.value = initial_value;
-  pinfo.descriptor = desc;
+  // Add declared parameters to storage.
+  parameters_out[name] = parameter_infos.at(name);
 
-  // Add declared parameters to storage, even when they're not set.
-  parameters_[name] = pinfo;
-
-  // If it has an actual value then add it to 'new_parameters' event
-  if (rclcpp::ParameterType::PARAMETER_NOT_SET != initial_value.get_type()) {
-    auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
-    parameter_event->new_parameters.push_back(
-      rclcpp::Parameter(name, initial_value).to_parameter_msg());
-    events_publisher_->publish(parameter_event);
+  // Extend the given parameter event, if valid.
+  if (parameter_event_out) {
+    parameter_event_out->new_parameters.push_back(parameter_wrappers[0].to_parameter_msg());
   }
+
+  return result;
+}
+
+const rclcpp::ParameterValue &
+NodeParameters::declare_parameter(
+  const std::string & name,
+  const rclcpp::ParameterValue & default_value,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  // TODO(sloretz) parameter name validation
+  if (name.empty()) {
+    throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty");
+  }
+
+  // Error if this parameter has already been declared and is different
+  if (__lockless_has_parameter(parameters_, name)) {
+    throw rclcpp::exceptions::ParameterAlreadyDeclaredException(
+            "parameter '" + name + "' has already been declared");
+  }
+
+  rcl_interfaces::msg::ParameterEvent parameter_event;
+  auto result = __declare_parameter_common(
+    name,
+    default_value,
+    parameter_descriptor,
+    parameters_,
+    initial_parameter_values_,
+    on_parameters_set_callback_,
+    &parameter_event);
+
+  // If it failed to be set, then throw an exception.
+  if (!result.successful) {
+    throw rclcpp::exceptions::InvalidParameterValueException(
+            "parameter '" + name + "' could not be set: " + result.reason);
+  }
+
+  // Publish the event.
+  events_publisher_->publish(parameter_event);
+
+  return parameters_.at(name).value;
+}
+
+void
+NodeParameters::undeclare_parameter(const std::string & name)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  auto parameter_info = parameters_.find(name);
+  if (parameter_info == parameters_.end()) {
+    throw rclcpp::exceptions::ParameterNotDeclaredException(
+            "cannot undeclare parameter '" + name + "' which has not yet been declared");
+  }
+
+  if (parameter_info->second.descriptor.read_only) {
+    throw rclcpp::exceptions::ParameterImmutableException(
+            "cannot undeclare parameter '" + name + "' because it is read-only");
+  }
+
+  parameters_.erase(parameter_info);
+}
+
+bool
+NodeParameters::has_parameter(const std::string & name) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  return __lockless_has_parameter(parameters_, name);
 }
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
-NodeParameters::set_parameters(
-  const std::vector<rclcpp::Parameter> & parameters)
+NodeParameters::set_parameters(const std::vector<rclcpp::Parameter> & parameters)
 {
   std::vector<rcl_interfaces::msg::SetParametersResult> results;
-  for (auto p : parameters) {
+  results.reserve(parameters.size());
+
+  for (const auto & p : parameters) {
     auto result = set_parameters_atomically({{p}});
     results.push_back(result);
   }
+
   return results;
 }
 
+template<typename ParameterVectorType>
+auto
+__find_parameter_by_name(
+  ParameterVectorType & parameters,
+  const std::string & name)
+{
+  return std::find_if(
+    parameters.begin(),
+    parameters.end(),
+    [&](auto parameter) {return parameter.get_name() == name;});
+}
+
 rcl_interfaces::msg::SetParametersResult
-NodeParameters::set_parameters_atomically(
-  const std::vector<rclcpp::Parameter> & parameters)
+NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters)
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  std::map<std::string, rclcpp::node_interfaces::ParameterInfo> tmp_map;
-  auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
 
-  parameter_event->node = combined_name_;
-
-  // TODO(jacquelinekay): handle parameter constraints
   rcl_interfaces::msg::SetParametersResult result;
-  if (parameters_callback_) {
-    result = parameters_callback_(parameters);
-  } else {
-    result.successful = true;
+
+  // Check if any of the parameters are read-only, or if any parameters are not
+  // declared.
+  // If not declared, keep track of them in order to declare them later, when
+  // undeclared parameters are allowed, and if they're not allowed, fail.
+  std::vector<const rclcpp::Parameter *> parameters_to_be_declared;
+  for (const auto & parameter : parameters) {
+    const std::string & name = parameter.get_name();
+
+    // Check to make sure the parameter name is valid.
+    if (name.empty()) {
+      throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty");
+    }
+
+    // Check to see if it is declared.
+    auto parameter_info = parameters_.find(name);
+    if (parameter_info == parameters_.end()) {
+      // If not check to see if undeclared paramaters are allowed, ...
+      if (allow_undeclared_) {
+        // If so, mark the parameter to be declared for the user implicitly.
+        parameters_to_be_declared.push_back(&parameter);
+        // continue as it cannot be read-only, and because the declare will
+        // implicitly set the parameter and parameter_infos is for setting only.
+        continue;
+      } else {
+        // If not, then throw the exception as documented.
+        throw rclcpp::exceptions::ParameterNotDeclaredException(
+                "parameter '" + name + "' cannot be set because it was not declared");
+      }
+    }
+
+    // Check to see if it is read-only.
+    if (parameter_info->second.descriptor.read_only) {
+      result.successful = false;
+      result.reason = "parameter '" + name + "' cannot be set because it is read-only";
+      return result;
+    }
   }
 
+  // Declare parameters into a temporary "staging area", incase one of the declares fail.
+  // We will use the staged changes as input to the "set atomically" action.
+  // We explicitly avoid calling the user callback here, so that it may be called once, with
+  // all the other parameters to be set (already declared parameters).
+  std::map<std::string, rclcpp::node_interfaces::ParameterInfo> staged_parameter_changes;
+  rcl_interfaces::msg::ParameterEvent parameter_event_msg;
+  parameter_event_msg.node = combined_name_;
+  for (auto parameter_to_be_declared : parameters_to_be_declared) {
+    // This should not throw, because we validated the name and checked that
+    // the parameter was not already declared.
+    result = __declare_parameter_common(
+      parameter_to_be_declared->get_name(),
+      parameter_to_be_declared->get_parameter_value(),
+      rcl_interfaces::msg::ParameterDescriptor(),  // Implicit declare uses default descriptor.
+      staged_parameter_changes,
+      initial_parameter_values_,
+      nullptr,  // callback is explicitly null, so that it is called only once, when setting below.
+      &parameter_event_msg);
+    if (!result.successful) {
+      // Declare failed, return knowing that nothing was changed because the
+      // staged changes were not applied.
+      return result;
+    }
+  }
+
+  // If there were implicitly declared parameters, then we may need to copy the input parameters
+  // and then assign the value that was selected after the declare (could be affected by the
+  // initial parameter values).
+  const std::vector<rclcpp::Parameter> * parameters_to_be_set = &parameters;
+  std::vector<rclcpp::Parameter> parameters_copy;
+  if (0 != staged_parameter_changes.size()) {  // If there were any implicitly declared parameters.
+    bool any_initial_values_used = false;
+    for (const auto & staged_parameter_change : staged_parameter_changes) {
+      auto it = __find_parameter_by_name(parameters, staged_parameter_change.first);
+      if (it->get_parameter_value() != staged_parameter_change.second.value) {
+        // In this case, the value of the staged parameter differs from the
+        // input from the user, and therefore we need to update things before setting.
+        any_initial_values_used = true;
+        // No need to search further since at least one initial value needs to be used.
+        break;
+      }
+    }
+    if (any_initial_values_used) {
+      parameters_copy = parameters;
+      for (const auto & staged_parameter_change : staged_parameter_changes) {
+        auto it = __find_parameter_by_name(parameters_copy, staged_parameter_change.first);
+        *it = Parameter(staged_parameter_change.first, staged_parameter_change.second.value);
+      }
+      parameters_to_be_set = &parameters_copy;
+    }
+  }
+
+  // Collect parameters who will have had their type changed to
+  // rclcpp::PARAMETER_NOT_SET so they can later be implicitly undeclared.
+  std::vector<const rclcpp::Parameter *> parameters_to_be_undeclared;
+  for (const auto & parameter : *parameters_to_be_set) {
+    if (rclcpp::PARAMETER_NOT_SET == parameter.get_type()) {
+      auto it = parameters_.find(parameter.get_name());
+      if (it != parameters_.end() && rclcpp::PARAMETER_NOT_SET != it->second.value.get_type()) {
+        parameters_to_be_undeclared.push_back(&parameter);
+      }
+    }
+  }
+
+  // Set the 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_);
+
+  // If not successful, then stop here.
   if (!result.successful) {
     return result;
   }
 
-  std::vector<std::string> to_delete;
+  // If successful, then update the parameter infos from the implicitly declared parameter's.
+  for (const auto & kv_pair : staged_parameter_changes) {
+    // assumption: the parameter is already present in parameters_ due to the above "set"
+    assert(__lockless_has_parameter(parameters_, kv_pair.first));
+    // assumption: the value in parameters_ is the same as the value resulting from the declare
+    assert(parameters_[kv_pair.first].value == kv_pair.second.value);
+    // This assignment should not change the name, type, or value, but may
+    // change other things from the ParameterInfo.
+    parameters_[kv_pair.first] = kv_pair.second;
+  }
 
-  for (auto p : parameters) {
-    auto param_iter = parameters_.find(p.get_name());
-    bool exists = parameters_.end() != param_iter;
-    bool want_to_delete = p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET;
-
-    if (exists) {
-      if (param_iter->second.descriptor.read_only) {
-        result.successful = false;
-        result.reason = "read_only parameter: '" + p.get_name() + "'";
-        return result;
-      }
-      if (want_to_delete) {
-        parameter_event->deleted_parameters.push_back(p.to_parameter_msg());
-        if (param_iter->second.is_declared) {
-          // clear declared parameter value, but don't delete it
-          rclcpp::node_interfaces::ParameterInfo cleared_param_info = param_iter->second;
-          cleared_param_info.value = rclcpp::ParameterValue();
-          tmp_map[p.get_name()] = cleared_param_info;
-        } else {
-          // Truly delete an undeclared parameter
-          to_delete.push_back(p.get_name());
-        }
-      } else {
-        if (param_iter->second.value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
-          // case: setting a value on a declared parameter that currently is unset
-          parameter_event->new_parameters.push_back(p.to_parameter_msg());
-        } else {
-          // case: changing a parameter value
-          parameter_event->changed_parameters.push_back(p.to_parameter_msg());
-        }
-        rclcpp::node_interfaces::ParameterInfo changed_param_info = param_iter->second;
-        // TODO(sloretz) Add accessor for ParameterValue on Parameter class
-        changed_param_info.value = rclcpp::ParameterValue(p.get_value_message());
-        tmp_map[p.get_name()] = changed_param_info;
-      }
-    } else {
-      // case: parameter does not exist already
-      if (!allow_undeclared_) {
-        result.successful = false;
-        result.reason = "undeclared parameter: '" + p.get_name() + "'";
-        return result;
-      } else if (want_to_delete) {
-        result.successful = false;
-        result.reason = "deleting parameter: '" + p.get_name() + "' that does not exist";
-        return result;
-      } else {
-        // Create new undeclared parameter
-        parameter_event->new_parameters.push_back(p.to_parameter_msg());
-
-        rcl_interfaces::msg::ParameterDescriptor desc;
-        desc.name = p.get_name();
-        desc.type = p.get_type();
-        desc.read_only = false;
-
-        rclcpp::node_interfaces::ParameterInfo new_param_info;
-        new_param_info.is_declared = false;
-        // TODO(sloretz) Add accessor for ParameterValue on Parameter class
-        new_param_info.value = rclcpp::ParameterValue(p.get_value_message());
-        new_param_info.descriptor = desc;
-
-        tmp_map[p.get_name()] = new_param_info;
-      }
+  // Undeclare parameters that need to be.
+  for (auto parameter_to_undeclare : parameters_to_be_undeclared) {
+    auto it = parameters_.find(parameter_to_undeclare->get_name());
+    // assumption: the parameter to be undeclared should be in the parameter infos map
+    assert(it != parameters_.end());
+    if (it != parameters_.end()) {
+      // Remove it and update the parameter event message.
+      parameters_.erase(it);
+      parameter_event_msg.deleted_parameters.push_back(
+        rclcpp::Parameter(it->first, it->second.value).to_parameter_msg());
     }
   }
-  // std::map::insert will not overwrite elements, so we'll keep the new
-  // ones and add only those that already exist in the Node's internal map
-  tmp_map.insert(parameters_.begin(), parameters_.end());
 
-  // remove truly deleted parameters
-  for (const std::string & param_name : to_delete) {
-    tmp_map.erase(param_name);
+  // Update the parameter event message for any parameters which were only set,
+  // and not either declared or undeclared.
+  for (const auto & parameter : *parameters_to_be_set) {
+    if (staged_parameter_changes.find(parameter.get_name()) != staged_parameter_changes.end()) {
+      // This parameter was declared.
+      continue;
+    }
+    auto it = std::find_if(
+      parameters_to_be_undeclared.begin(),
+      parameters_to_be_undeclared.end(),
+      [&parameter](const auto & p) {return p->get_name() == parameter.get_name();});
+    if (it != parameters_to_be_undeclared.end()) {
+      // This parameter was undeclared (deleted).
+      continue;
+    }
+    // This parameter was neither declared nor undeclared.
+    parameter_event_msg.changed_parameters.push_back(parameter.to_parameter_msg());
   }
 
-  std::swap(tmp_map, parameters_);
-  parameter_event->stamp = node_clock_->get_clock()->now();
-  events_publisher_->publish(parameter_event);
+  // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor.
+  if (nullptr != events_publisher_) {
+    parameter_event_msg.stamp = node_clock_->get_clock()->now();
+    events_publisher_->publish(parameter_event_msg);
+  }
 
   return result;
 }
@@ -335,14 +541,19 @@ NodeParameters::get_parameters(const std::vector<std::string> & names) const
 {
   std::lock_guard<std::mutex> lock(mutex_);
   std::vector<rclcpp::Parameter> results;
+  results.reserve(names.size());
 
   for (auto & name : names) {
-    if (std::any_of(parameters_.cbegin(), parameters_.cend(),
-      [&name](const std::pair<std::string, rclcpp::node_interfaces::ParameterInfo> & kv) {
-        return name == kv.first;
-      }))
-    {
-      results.emplace_back(name, parameters_.at(name).value);
+    auto found_parameter = parameters_.find(name);
+    if (found_parameter != parameters_.cend()) {
+      // found
+      results.emplace_back(name, found_parameter->second.value);
+    } else if (this->allow_undeclared_) {
+      // not found, but undeclared allowed
+      results.emplace_back(name, rclcpp::ParameterValue());
+    } else {
+      // not found, and undeclared are not allowed
+      throw rclcpp::exceptions::ParameterNotDeclaredException(name);
     }
   }
   return results;
@@ -355,8 +566,10 @@ NodeParameters::get_parameter(const std::string & name) const
 
   if (get_parameter(name, parameter)) {
     return parameter;
+  } else if (this->allow_undeclared_) {
+    return parameter;
   } else {
-    throw std::out_of_range("Parameter '" + name + "' not set");
+    throw rclcpp::exceptions::ParameterNotDeclaredException(name);
   }
 }
 
@@ -384,11 +597,11 @@ NodeParameters::get_parameters_by_prefix(
   const std::string & prefix,
   std::map<std::string, rclcpp::Parameter> & parameters) const
 {
-  std::string prefix_with_dot = prefix + ".";
-  bool ret = false;
-
   std::lock_guard<std::mutex> lock(mutex_);
 
+  std::string prefix_with_dot = prefix.empty() ? prefix : prefix + ".";
+  bool ret = false;
+
   for (const auto & param : parameters_) {
     if (param.first.find(prefix_with_dot) == 0 && param.first.length() > prefix_with_dot.length()) {
       // Found one!
@@ -405,14 +618,26 @@ NodeParameters::describe_parameters(const std::vector<std::string> & names) cons
 {
   std::lock_guard<std::mutex> lock(mutex_);
   std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
-  for (auto & kv : parameters_) {
-    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-        return name == kv.first;
-      }))
-    {
-      results.push_back(kv.second.descriptor);
+  results.reserve(names.size());
+
+  for (const auto & name : names) {
+    auto it = parameters_.find(name);
+    if (it != parameters_.cend()) {
+      results.push_back(it->second.descriptor);
+    } else if (allow_undeclared_) {
+      // parameter not found, but undeclared allowed, so return empty
+      rcl_interfaces::msg::ParameterDescriptor default_description;
+      default_description.name = name;
+      results.push_back(default_description);
+    } else {
+      throw rclcpp::exceptions::ParameterNotDeclaredException(name);
     }
   }
+
+  if (results.size() != names.size()) {
+    throw std::runtime_error("results and names unexpectedly different sizes");
+  }
+
   return results;
 }
 
@@ -421,16 +646,24 @@ NodeParameters::get_parameter_types(const std::vector<std::string> & names) cons
 {
   std::lock_guard<std::mutex> lock(mutex_);
   std::vector<uint8_t> results;
-  for (auto & kv : parameters_) {
-    if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) {
-        return name == kv.first;
-      }))
-    {
-      results.push_back(kv.second.value.get_type());
-    } else {
+  results.reserve(names.size());
+
+  for (const auto & name : names) {
+    auto it = parameters_.find(name);
+    if (it != parameters_.cend()) {
+      results.push_back(it->second.value.get_type());
+    } else if (allow_undeclared_) {
+      // parameter not found, but undeclared allowed, so return not set
       results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
+    } else {
+      throw rclcpp::exceptions::ParameterNotDeclaredException(name);
     }
   }
+
+  if (results.size() != names.size()) {
+    throw std::runtime_error("results and names unexpectedly different sizes");
+  }
+
   return results;
 }
 
@@ -476,12 +709,39 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
   return result;
 }
 
+NodeParameters::OnParametersSetCallbackType
+NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback)
+{
+  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 (parameters_callback_) {
-    RCUTILS_LOG_WARN("param_change_callback already registered, "
-      "overwriting previous callback");
+  if (on_parameters_set_callback_) {
+    RCLCPP_WARN(
+      node_logging_->get_logger(),
+      "on_parameters_set_callback already registered, overwriting previous callback");
   }
-  parameters_callback_ = 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_initial_parameter_values() const
+{
+  return initial_parameter_values_;
 }

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -54,6 +54,7 @@ NodeTopics::create_publisher(
     publisher->setup_intra_process(
       intra_process_publisher_id,
       shared_publish_callback,
+      ipm,
       publisher_options);
   }
 

rclcpp/src/rclcpp/node_options.cpp

@@ -0,0 +1,294 @@
+// 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/node_options.hpp"
+
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+
+using rclcpp::exceptions::throw_from_rcl_error;
+
+namespace rclcpp
+{
+
+namespace detail
+{
+static
+void
+rcl_node_options_t_destructor(rcl_node_options_t * node_options)
+{
+  if (node_options) {
+    rcl_ret_t ret = rcl_node_options_fini(node_options);
+    if (RCL_RET_OK != ret) {
+      // Cannot throw here, as it may be called in the destructor.
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "failed to finalize rcl node options: %s", rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+  }
+}
+}  // namespace detail
+
+NodeOptions::NodeOptions(rcl_allocator_t allocator)
+: node_options_(nullptr, detail::rcl_node_options_t_destructor), allocator_(allocator)
+{}
+
+NodeOptions::NodeOptions(const NodeOptions & other)
+: node_options_(nullptr, detail::rcl_node_options_t_destructor)
+{
+  *this = other;
+}
+
+NodeOptions &
+NodeOptions::operator=(const NodeOptions & other)
+{
+  if (this != &other) {
+    this->context_ = other.context_;
+    this->arguments_ = other.arguments_;
+    this->initial_parameters_ = other.initial_parameters_;
+    this->use_global_arguments_ = other.use_global_arguments_;
+    this->use_intra_process_comms_ = other.use_intra_process_comms_;
+    this->start_parameter_services_ = other.start_parameter_services_;
+    this->allocator_ = other.allocator_;
+  }
+  return *this;
+}
+
+const rcl_node_options_t *
+NodeOptions::get_rcl_node_options() const
+{
+  // If it is nullptr, create it on demand.
+  if (!node_options_) {
+    node_options_.reset(new rcl_node_options_t);
+    *node_options_ = rcl_node_get_default_options();
+    node_options_->allocator = this->allocator_;
+    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;
+    if (!this->arguments_.empty()) {
+      c_args.reset(new const char *[this->arguments_.size()]);
+      for (std::size_t i = 0; i < this->arguments_.size(); ++i) {
+        c_args[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));
+
+    if (RCL_RET_OK != ret) {
+      throw_from_rcl_error(ret, "failed to parse arguments");
+    }
+  }
+
+  return node_options_.get();
+}
+
+rclcpp::Context::SharedPtr
+NodeOptions::context() const
+{
+  return this->context_;
+}
+
+NodeOptions &
+NodeOptions::context(rclcpp::Context::SharedPtr context)
+{
+  this->context_ = context;
+  return *this;
+}
+
+const std::vector<std::string> &
+NodeOptions::arguments() const
+{
+  return this->arguments_;
+}
+
+NodeOptions &
+NodeOptions::arguments(const std::vector<std::string> & arguments)
+{
+  this->node_options_.reset();  // reset node options to make it be recreated on next access.
+  this->arguments_ = arguments;
+  return *this;
+}
+
+std::vector<rclcpp::Parameter> &
+NodeOptions::initial_parameters()
+{
+  return this->initial_parameters_;
+}
+
+const std::vector<rclcpp::Parameter> &
+NodeOptions::initial_parameters() const
+{
+  return this->initial_parameters_;
+}
+
+NodeOptions &
+NodeOptions::initial_parameters(const std::vector<rclcpp::Parameter> & initial_parameters)
+{
+  this->initial_parameters_ = initial_parameters;
+  return *this;
+}
+
+bool
+NodeOptions::use_global_arguments() const
+{
+  return this->node_options_->use_global_arguments;
+}
+
+NodeOptions &
+NodeOptions::use_global_arguments(bool use_global_arguments)
+{
+  this->node_options_.reset();  // reset node options to make it be recreated on next access.
+  this->use_global_arguments_ = use_global_arguments;
+  return *this;
+}
+
+bool
+NodeOptions::use_intra_process_comms() const
+{
+  return this->use_intra_process_comms_;
+}
+
+NodeOptions &
+NodeOptions::use_intra_process_comms(bool use_intra_process_comms)
+{
+  this->use_intra_process_comms_ = use_intra_process_comms;
+  return *this;
+}
+
+bool
+NodeOptions::start_parameter_services() const
+{
+  return this->start_parameter_services_;
+}
+
+NodeOptions &
+NodeOptions::start_parameter_services(bool start_parameter_services)
+{
+  this->start_parameter_services_ = start_parameter_services;
+  return *this;
+}
+
+bool
+NodeOptions::start_parameter_event_publisher() const
+{
+  return this->start_parameter_event_publisher_;
+}
+
+NodeOptions &
+NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publisher)
+{
+  this->start_parameter_event_publisher_ = start_parameter_event_publisher;
+  return *this;
+}
+
+const rmw_qos_profile_t &
+NodeOptions::parameter_event_qos_profile() const
+{
+  return this->parameter_event_qos_profile_;
+}
+
+NodeOptions &
+NodeOptions::parameter_event_qos_profile(const rmw_qos_profile_t & parameter_event_qos_profile)
+{
+  this->parameter_event_qos_profile_ = parameter_event_qos_profile;
+  return *this;
+}
+
+bool
+NodeOptions::allow_undeclared_parameters() const
+{
+  return this->allow_undeclared_parameters_;
+}
+
+NodeOptions &
+NodeOptions::allow_undeclared_parameters(bool allow_undeclared_parameters)
+{
+  this->allow_undeclared_parameters_ = allow_undeclared_parameters;
+  return *this;
+}
+
+bool
+NodeOptions::automatically_declare_initial_parameters() const
+{
+  return this->automatically_declare_initial_parameters_;
+}
+
+NodeOptions &
+NodeOptions::automatically_declare_initial_parameters(
+  bool automatically_declare_initial_parameters)
+{
+  this->automatically_declare_initial_parameters_ = automatically_declare_initial_parameters;
+  return *this;
+}
+
+const rcl_allocator_t &
+NodeOptions::allocator() const
+{
+  return this->allocator_;
+}
+
+NodeOptions &
+NodeOptions::allocator(rcl_allocator_t allocator)
+{
+  this->node_options_.reset();  // reset node options to make it be recreated on next access.
+  this->allocator_ = allocator;
+  return *this;
+}
+
+// TODO(wjwwood): reuse rcutils_get_env() to avoid code duplication.
+//   See also: https://github.com/ros2/rcl/issues/119
+size_t
+NodeOptions::get_domain_id_from_env() const
+{
+  // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable.
+  size_t domain_id = std::numeric_limits<size_t>::max();
+  char * ros_domain_id = nullptr;
+  const char * env_var = "ROS_DOMAIN_ID";
+#ifndef _WIN32
+  ros_domain_id = getenv(env_var);
+#else
+  size_t ros_domain_id_size;
+  _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
+#endif
+  if (ros_domain_id) {
+    uint32_t number = strtoul(ros_domain_id, NULL, 0);
+    if (number == (std::numeric_limits<uint32_t>::max)()) {
+#ifdef _WIN32
+      // free the ros_domain_id before throwing, if getenv was used on Windows
+      free(ros_domain_id);
+#endif
+      throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
+    }
+    domain_id = static_cast<size_t>(number);
+#ifdef _WIN32
+    free(ros_domain_id);
+#endif
+  }
+  return domain_id;
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/parameter.cpp

@@ -19,7 +19,17 @@
 #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;
@@ -30,6 +40,11 @@ Parameter::Parameter()
 {
 }
 
+Parameter::Parameter(const std::string & name)
+: name_(name), value_()
+{
+}
+
 Parameter::Parameter(const std::string & name, const rclcpp::ParameterValue & value)
 : name_(name), value_(value)
 {
@@ -40,6 +55,18 @@ Parameter::Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_in
 {
 }
 
+bool
+Parameter::operator==(const Parameter & rhs) const
+{
+  return this->name_ == rhs.name_ && this->value_ == rhs.value_;
+}
+
+bool
+Parameter::operator!=(const Parameter & rhs) const
+{
+  return !(*this == rhs);
+}
+
 ParameterType
 Parameter::get_type() const
 {

rclcpp/src/rclcpp/parameter_value.cpp

@@ -227,3 +227,15 @@ ParameterValue::to_value_msg() const
 {
   return value_;
 }
+
+bool
+ParameterValue::operator==(const ParameterValue & rhs) const
+{
+  return this->value_ == rhs.value_;
+}
+
+bool
+ParameterValue::operator!=(const ParameterValue & rhs) const
+{
+  return this->value_ != rhs.value_;
+}

rclcpp/src/rclcpp/publisher.cpp

@@ -30,6 +30,7 @@
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
@@ -42,7 +43,8 @@ PublisherBase::PublisherBase(
   const rosidl_message_type_support_t & type_support,
   const rcl_publisher_options_t & publisher_options)
 : rcl_node_handle_(node_base->get_shared_rcl_node_handle()),
-  intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
+  intra_process_is_enabled_(false), intra_process_publisher_id_(0),
+  store_intra_process_message_(nullptr)
 {
   rcl_ret_t ret = rcl_publisher_init(
     &publisher_handle_,
@@ -94,6 +96,20 @@ PublisherBase::~PublisherBase()
       rcl_get_error_string().str);
     rcl_reset_error();
   }
+
+  auto ipm = weak_ipm_.lock();
+
+  if (!intra_process_is_enabled_) {
+    return;
+  }
+  if (!ipm) {
+    // TODO(ivanpauno): should this raise an error?
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Intra process manager died before than a publisher.");
+    return;
+  }
+  ipm->remove_publisher(intra_process_publisher_id_);
 }
 
 const char *
@@ -138,6 +154,60 @@ PublisherBase::get_publisher_handle() const
   return &publisher_handle_;
 }
 
+size_t
+PublisherBase::get_subscription_count() const
+{
+  size_t inter_process_subscription_count = 0;
+
+  rcl_ret_t status = rcl_publisher_get_subscription_count(
+    &publisher_handle_,
+    &inter_process_subscription_count);
+
+  if (RCL_RET_PUBLISHER_INVALID == status) {
+    rcl_reset_error();  /* next call will reset error message if not context */
+    if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
+      rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
+      if (nullptr != context && !rcl_context_is_valid(context)) {
+        /* publisher is invalid due to context being shutdown */
+        return 0;
+      }
+    }
+  }
+  if (RCL_RET_OK != status) {
+    rclcpp::exceptions::throw_from_rcl_error(status, "failed to get get subscription count");
+  }
+  return inter_process_subscription_count;
+}
+
+size_t
+PublisherBase::get_intra_process_subscription_count() const
+{
+  auto ipm = weak_ipm_.lock();
+  if (!intra_process_is_enabled_) {
+    return 0;
+  }
+  if (!ipm) {
+    // TODO(ivanpauno): should this just return silently? Or maybe return with a warning?
+    //                  Same as wjwwood comment in publisher_factory create_shared_publish_callback.
+    throw std::runtime_error(
+            "intra process subscriber count called after "
+            "destruction of intra process manager");
+  }
+  return ipm->get_subscription_count(intra_process_publisher_id_);
+}
+
+rmw_qos_profile_t
+PublisherBase::get_actual_qos() const
+{
+  const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(&publisher_handle_);
+  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 *qos;
+}
+
 bool
 PublisherBase::operator==(const rmw_gid_t & gid) const
 {
@@ -168,9 +238,15 @@ PublisherBase::operator==(const rmw_gid_t * gid) const
 void
 PublisherBase::setup_intra_process(
   uint64_t intra_process_publisher_id,
-  StoreMessageCallbackT callback,
+  StoreMessageCallbackT store_callback,
+  IntraProcessManagerSharedPtr ipm,
   const rcl_publisher_options_t & intra_process_options)
 {
+  // Intraprocess configuration is not allowed with "durability" qos policy non "volatile".
+  if (this->get_actual_qos().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) {
+    throw exceptions::InvalidParametersException(
+            "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");
@@ -199,7 +275,10 @@ PublisherBase::setup_intra_process(
   }
 
   intra_process_publisher_id_ = intra_process_publisher_id;
-  store_intra_process_message_ = callback;
+  store_intra_process_message_ = store_callback;
+  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_);

rclcpp/src/rclcpp/signal_handler.cpp

@@ -46,18 +46,38 @@ dispatch_semaphore_t SignalHandler::signal_handler_sem_;
 sem_t SignalHandler::signal_handler_sem_;
 #endif
 
-SignalHandler &
-SignalHandler::get_global_signal_handler()
-{
-  static SignalHandler signal_handler;
-  return signal_handler;
-}
+// The logger must be initialized before the local static variable signal_handler,
+// from the method get_global_signal_handler(), so that it is destructed after
+// it, because the destructor of SignalHandler uses this logger object.
+static rclcpp::Logger g_logger = rclcpp::get_logger("rclcpp");
 
 rclcpp::Logger &
 SignalHandler::get_logger()
 {
-  static rclcpp::Logger logger = rclcpp::get_logger("rclcpp");
-  return logger;
+  return g_logger;
+}
+
+SignalHandler &
+SignalHandler::get_global_signal_handler()
+{
+  // This is initialized after the g_logger static global, ensuring
+  // SignalHandler::get_logger() may be called from the destructor of
+  // SignalHandler, according to this:
+  //
+  //   Variables declared at block scope with the specifier static have static
+  //   storage duration but are initialized the first time control passes
+  //   through their declaration (unless their initialization is zero- or
+  //   constant-initialization, which can be performed before the block is
+  //   first entered). On all further calls, the declaration is skipped.
+  //
+  // -- https://en.cppreference.com/w/cpp/language/storage_duration#Static_local_variables
+  //
+  // Which is guaranteed to occur after static initialization for global (see:
+  // https://en.cppreference.com/w/cpp/language/initialization#Static_initialization),
+  // which is when g_logger will be initialized.
+  // And destruction will occur in the reverse order.
+  static SignalHandler signal_handler;
+  return signal_handler;
 }
 
 bool

rclcpp/src/rclcpp/subscription.cpp

@@ -20,6 +20,7 @@
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
+#include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/logging.hpp"
 
 #include "rmw/error_handling.h"
@@ -34,6 +35,8 @@ SubscriptionBase::SubscriptionBase(
   const rcl_subscription_options_t & subscription_options,
   bool is_serialized)
 : node_handle_(node_handle),
+  use_intra_process_(false),
+  intra_process_subscription_id_(0),
   type_support_(type_support_handle),
   is_serialized_(is_serialized)
 {
@@ -80,6 +83,18 @@ SubscriptionBase::SubscriptionBase(
 
 SubscriptionBase::~SubscriptionBase()
 {
+  if (!use_intra_process_) {
+    return;
+  }
+  auto ipm = weak_ipm_.lock();
+  if (!ipm) {
+    // TODO(ivanpauno): should this raise an error?
+    RCLCPP_WARN(
+      rclcpp::get_logger("rclcpp"),
+      "Intra process manager died before than a subscription.");
+    return;
+  }
+  ipm->remove_subscription(intra_process_subscription_id_);
 }
 
 const char *
@@ -117,3 +132,18 @@ SubscriptionBase::is_serialized() const
 {
   return is_serialized_;
 }
+
+size_t
+SubscriptionBase::get_publisher_count() const
+{
+  size_t inter_process_publisher_count = 0;
+
+  rmw_ret_t status = rcl_subscription_get_publisher_count(
+    subscription_handle_.get(),
+    &inter_process_publisher_count);
+
+  if (RCL_RET_OK != status) {
+    rclcpp::exceptions::throw_from_rcl_error(status, "failed to get get publisher count");
+  }
+  return inter_process_publisher_count;
+}

rclcpp/src/rclcpp/time_source.cpp

@@ -78,20 +78,29 @@ void TimeSource::attachNode(
 
   logger_ = node_logging_->get_logger();
 
-  rclcpp::Parameter use_sim_time_param;
-  if (node_parameters_->get_parameter("use_sim_time", use_sim_time_param)) {
-    if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
-      if (use_sim_time_param.get_value<bool>() == true) {
-        parameter_state_ = SET_TRUE;
-        enable_ros_time();
-        create_clock_sub();
-      }
-    } else {
-      RCLCPP_ERROR(logger_, "Invalid type for parameter 'use_sim_time' %s should be bool",
-        use_sim_time_param.get_type_name().c_str());
+  // 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";
+  if (!node_parameters_->has_parameter(use_sim_time_name)) {
+    use_sim_time_param = node_parameters_->declare_parameter(
+      use_sim_time_name,
+      rclcpp::ParameterValue(false),
+      rcl_interfaces::msg::ParameterDescriptor());
+  } else {
+    use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value();
+  }
+  if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
+    if (use_sim_time_param.get<bool>()) {
+      parameter_state_ = SET_TRUE;
+      enable_ros_time();
+      create_clock_sub();
     }
   } else {
-    RCLCPP_DEBUG(logger_, "'use_sim_time' parameter not set, using wall time by default.");
+    // 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::to_string(use_sim_time_param.get_type()).c_str());
   }
 
   // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609

rclcpp/src/rclcpp/timer.cpp

@@ -19,6 +19,8 @@
 #include <memory>
 
 #include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/exceptions.hpp"
+
 #include "rcutils/logging_macros.h"
 
 using rclcpp::TimerBase;
@@ -75,6 +77,17 @@ TimerBase::cancel()
   }
 }
 
+bool
+TimerBase::is_canceled()
+{
+  bool is_canceled = false;
+  rcl_ret_t ret = rcl_timer_is_canceled(timer_handle_.get(), &is_canceled);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get timer cancelled state");
+  }
+  return is_canceled;
+}
+
 void
 TimerBase::reset()
 {

rclcpp/src/rclcpp/utilities.cpp

@@ -24,45 +24,48 @@
 #include "rcl/error_handling.h"
 #include "rcl/rcl.h"
 
-void
-rclcpp::init(int argc, char const * const argv[], const rclcpp::InitOptions & init_options)
+namespace rclcpp
 {
-  using rclcpp::contexts::default_context::get_global_default_context;
+
+void
+init(int argc, char const * const argv[], const InitOptions & init_options)
+{
+  using contexts::default_context::get_global_default_context;
   get_global_default_context()->init(argc, argv, init_options);
   // Install the signal handlers.
-  rclcpp::install_signal_handlers();
+  install_signal_handlers();
 }
 
 bool
-rclcpp::install_signal_handlers()
+install_signal_handlers()
 {
-  return rclcpp::SignalHandler::get_global_signal_handler().install();
+  return SignalHandler::get_global_signal_handler().install();
 }
 
 bool
-rclcpp::signal_handlers_installed()
+signal_handlers_installed()
 {
-  return rclcpp::SignalHandler::get_global_signal_handler().is_installed();
+  return SignalHandler::get_global_signal_handler().is_installed();
 }
 
 bool
-rclcpp::uninstall_signal_handlers()
+uninstall_signal_handlers()
 {
-  return rclcpp::SignalHandler::get_global_signal_handler().uninstall();
+  return SignalHandler::get_global_signal_handler().uninstall();
 }
 
 std::vector<std::string>
-rclcpp::init_and_remove_ros_arguments(
+init_and_remove_ros_arguments(
   int argc,
   char const * const argv[],
-  const rclcpp::InitOptions & init_options)
+  const InitOptions & init_options)
 {
-  rclcpp::init(argc, argv, init_options);
-  return rclcpp::remove_ros_arguments(argc, argv);
+  init(argc, argv, init_options);
+  return remove_ros_arguments(argc, argv);
 }
 
 std::vector<std::string>
-rclcpp::remove_ros_arguments(int argc, char const * const argv[])
+remove_ros_arguments(int argc, char const * const argv[])
 {
   rcl_allocator_t alloc = rcl_get_default_allocator();
   rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
@@ -71,7 +74,7 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
 
   ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
   if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to parse arguments");
+    exceptions::throw_from_rcl_error(ret, "failed to parse arguments");
   }
 
   int nonros_argc = 0;
@@ -84,9 +87,9 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
     &nonros_argc,
     &nonros_argv);
 
-  if (RCL_RET_OK != ret) {
+  if (RCL_RET_OK != ret || nonros_argc < 0) {
     // Not using throw_from_rcl_error, because we may need to append deallocation failures.
-    rclcpp::exceptions::RCLErrorBase base_exc(ret, rcl_get_error_state());
+    exceptions::RCLErrorBase base_exc(ret, rcl_get_error_state());
     rcl_reset_error();
     if (NULL != nonros_argv) {
       alloc.deallocate(nonros_argv, alloc.state);
@@ -97,11 +100,10 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
         rcl_get_error_string().str;
       rcl_reset_error();
     }
-    throw rclcpp::exceptions::RCLError(base_exc, "");
+    throw exceptions::RCLError(base_exc, "");
   }
 
-  std::vector<std::string> return_arguments;
-  return_arguments.resize(nonros_argc);
+  std::vector<std::string> return_arguments(nonros_argc);
 
   for (int ii = 0; ii < nonros_argc; ++ii) {
     return_arguments[ii] = std::string(nonros_argv[ii]);
@@ -113,7 +115,7 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
 
   ret = rcl_arguments_fini(&parsed_args);
   if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(
+    exceptions::throw_from_rcl_error(
       ret, "failed to cleanup parsed arguments, leaking memory");
   }
 
@@ -121,9 +123,9 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
 }
 
 bool
-rclcpp::ok(rclcpp::Context::SharedPtr context)
+ok(Context::SharedPtr context)
 {
-  using rclcpp::contexts::default_context::get_global_default_context;
+  using contexts::default_context::get_global_default_context;
   if (nullptr == context) {
     context = get_global_default_context();
   }
@@ -131,30 +133,30 @@ rclcpp::ok(rclcpp::Context::SharedPtr context)
 }
 
 bool
-rclcpp::is_initialized(rclcpp::Context::SharedPtr context)
+is_initialized(Context::SharedPtr context)
 {
-  return rclcpp::ok(context);
+  return ok(context);
 }
 
 bool
-rclcpp::shutdown(rclcpp::Context::SharedPtr context, const std::string & reason)
+shutdown(Context::SharedPtr context, const std::string & reason)
 {
-  using rclcpp::contexts::default_context::get_global_default_context;
+  using contexts::default_context::get_global_default_context;
   auto default_context = get_global_default_context();
   if (nullptr == context) {
     context = default_context;
   }
   bool ret = context->shutdown(reason);
   if (context == default_context) {
-    rclcpp::uninstall_signal_handlers();
+    uninstall_signal_handlers();
   }
   return ret;
 }
 
 void
-rclcpp::on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr context)
+on_shutdown(std::function<void()> callback, Context::SharedPtr context)
 {
-  using rclcpp::contexts::default_context::get_global_default_context;
+  using contexts::default_context::get_global_default_context;
   if (nullptr == context) {
     context = get_global_default_context();
   }
@@ -162,9 +164,9 @@ rclcpp::on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr c
 }
 
 bool
-rclcpp::sleep_for(const std::chrono::nanoseconds & nanoseconds, rclcpp::Context::SharedPtr context)
+sleep_for(const std::chrono::nanoseconds & nanoseconds, Context::SharedPtr context)
 {
-  using rclcpp::contexts::default_context::get_global_default_context;
+  using contexts::default_context::get_global_default_context;
   if (nullptr == context) {
     context = get_global_default_context();
   }
@@ -172,13 +174,15 @@ rclcpp::sleep_for(const std::chrono::nanoseconds & nanoseconds, rclcpp::Context:
 }
 
 const char *
-rclcpp::get_c_string(const char * string_in)
+get_c_string(const char * string_in)
 {
   return string_in;
 }
 
 const char *
-rclcpp::get_c_string(const std::string & string_in)
+get_c_string(const std::string & string_in)
 {
   return string_in.c_str();
 }
+
+}  // namespace rclcpp

rclcpp/test/test_client.cpp

@@ -43,6 +43,28 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
+class TestClientSub : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    subnode = node->create_sub_node("sub_ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr subnode;
+};
+
 /*
    Testing client construction and destruction.
  */
@@ -58,3 +80,20 @@ TEST_F(TestClient, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }
+
+/*
+   Testing client construction and destruction for subnodes.
+ */
+TEST_F(TestClientSub, construction_and_destruction) {
+  using rcl_interfaces::srv::ListParameters;
+  {
+    auto client = subnode->create_client<ListParameters>("service");
+    EXPECT_STREQ(client->get_service_name(), "/ns/sub_ns/service");
+  }
+
+  {
+    ASSERT_THROW({
+      auto client = node->create_client<ListParameters>("invalid_service?");
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}

rclcpp/test/test_local_parameters.cpp

@@ -21,7 +21,9 @@
 #include "rclcpp/rclcpp.hpp"
 
 TEST(test_local_parameters, set_parameter_if_not_set) {
-  auto node = rclcpp::Node::make_shared("test_local_parameters_set_parameter_if_not_set");
+  auto node = rclcpp::Node::make_shared(
+    "test_local_parameters_set_parameter_if_not_set",
+    rclcpp::NodeOptions().allow_undeclared_parameters(true));
 
   {
     // try to set a map of parameters
@@ -29,7 +31,19 @@ TEST(test_local_parameters, set_parameter_if_not_set) {
       {"x", 0.5},
       {"y", 1.0},
     };
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else  // !defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
     node->set_parameters_if_not_set("bar", bar_map);
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else  // !defined(_WIN32)
+# pragma warning(pop)
+#endif
     double bar_x_value;
     ASSERT_TRUE(node->get_parameter("bar.x", bar_x_value));
     EXPECT_EQ(bar_x_value, 0.5);
@@ -51,8 +65,20 @@ TEST(test_local_parameters, set_parameter_if_not_set) {
 
   {
     // set parameters for a map with different types, then try to get them back as a map
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else  // !defined(_WIN32)
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
     node->set_parameter_if_not_set("baz.x", 1.0);
     node->set_parameter_if_not_set("baz.y", "hello");
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else  // !defined(_WIN32)
+# pragma warning(pop)
+#endif
     std::map<std::string, double> baz_map;
     EXPECT_THROW(node->get_parameters("baz", baz_map), rclcpp::ParameterTypeException);
   }

rclcpp/test/test_node_global_args.cpp

@@ -20,6 +20,7 @@
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/node.hpp"
+#include "rclcpp/node_options.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 class TestNodeWithGlobalArgs : public ::testing::Test
@@ -33,32 +34,26 @@ protected:
 };
 
 TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) {
-  auto context = rclcpp::contexts::default_context::get_global_default_context();
-  const std::vector<std::string> arguments = {"__node:=local_arguments_test"};
-  const std::vector<rclcpp::Parameter> initial_values = {};
-  const bool use_global_arguments = true;
-  const bool use_intra_process = false;
-  auto node = rclcpp::Node::make_shared(
-    "orig_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process);
+  auto options = rclcpp::NodeOptions()
+    .arguments({"__node:=local_arguments_test"});
+
+  auto node = rclcpp::Node::make_shared("orig_name", options);
   EXPECT_STREQ("local_arguments_test", node->get_name());
 }
 
 TEST_F(TestNodeWithGlobalArgs, use_or_ignore_global_arguments) {
-  auto context = rclcpp::contexts::default_context::get_global_default_context();
-  const std::vector<std::string> arguments = {};
-  const std::vector<rclcpp::Parameter> initial_values = {};
-  const bool use_intra_process = false;
-
   {  // Don't use global args
-    const bool use_global_arguments = false;
-    auto node = rclcpp::Node::make_shared(
-      "orig_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process);
+    auto options = rclcpp::NodeOptions()
+      .use_global_arguments(false);
+
+    auto node = rclcpp::Node::make_shared("orig_name", options);
     EXPECT_STREQ("orig_name", node->get_name());
   }
   {  // Do use global args
-    const bool use_global_arguments = true;
-    auto node = rclcpp::Node::make_shared(
-      "orig_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process);
+    auto options = rclcpp::NodeOptions()
+      .use_global_arguments(true);
+
+    auto node = rclcpp::Node::make_shared("orig_name", options);
     EXPECT_STREQ("global_node_name", node->get_name());
   }
 }

rclcpp/test/test_node_initial_parameters.cpp

@@ -1,117 +0,0 @@
-// Copyright 2018 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <gtest/gtest.h>
-
-#include <string>
-#include <memory>
-#include <vector>
-
-#include "rclcpp/node.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-class TestNodeWithInitialValues : public ::testing::Test
-{
-protected:
-  static void SetUpTestCase()
-  {
-    rclcpp::init(0, NULL);
-  }
-
-  static void TearDownTestCase()
-  {
-    rclcpp::shutdown();
-  }
-};
-
-TEST_F(TestNodeWithInitialValues, no_initial_values) {
-  auto context = rclcpp::contexts::default_context::get_global_default_context();
-  const std::vector<std::string> arguments = {};
-  const std::vector<rclcpp::Parameter> initial_values = {};
-  const bool use_global_arguments = false;
-  const bool use_intra_process = false;
-  auto node = rclcpp::Node::make_shared(
-    "node_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process);
-  auto list_params_result = node->list_parameters({}, 0);
-  // Has use_sim_time parameter
-  EXPECT_EQ(1u, list_params_result.names.size());
-}
-
-TEST_F(TestNodeWithInitialValues, multiple_undeclared_initial_values) {
-  auto context = rclcpp::contexts::default_context::get_global_default_context();
-  const std::vector<std::string> arguments = {};
-  const std::vector<rclcpp::Parameter> initial_values = {
-    rclcpp::Parameter("foo", true),
-    rclcpp::Parameter("bar", "hello world"),
-    rclcpp::Parameter("baz", std::vector<double>{3.14, 2.718})
-  };
-  const bool use_global_arguments = false;
-  const bool use_intra_process = false;
-  auto node = rclcpp::Node::make_shared(
-    "node_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process);
-  auto list_params_result = node->list_parameters({}, 0);
-  // Has use_sim_time parameter
-  EXPECT_EQ(1u, list_params_result.names.size());
-}
-
-TEST_F(TestNodeWithInitialValues, multiple_declared_initial_values) {
-  auto context = rclcpp::contexts::default_context::get_global_default_context();
-  const std::vector<std::string> arguments = {};
-  const std::vector<rclcpp::Parameter> initial_values = {
-    rclcpp::Parameter("foo", true),
-    rclcpp::Parameter("bar", "hello world"),
-    rclcpp::Parameter("baz", std::vector<double>{3.14, 2.718})
-  };
-  const bool use_global_arguments = false;
-  const bool use_intra_process = false;
-  auto node = rclcpp::Node::make_shared(
-    "node_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process);
-
-  node->declare_parameter("foo");
-  node->declare_parameter("bar");
-  node->declare_parameter("baz");
-
-  auto list_params_result = node->list_parameters({}, 0);
-  EXPECT_TRUE(node->get_parameter("foo").get_value<bool>());
-  EXPECT_STREQ("hello world", node->get_parameter("bar").get_value<std::string>().c_str());
-  std::vector<double> double_array = node->get_parameter("baz").get_value<std::vector<double>>();
-  ASSERT_EQ(2u, double_array.size());
-  EXPECT_DOUBLE_EQ(3.14, double_array.at(0));
-  EXPECT_DOUBLE_EQ(2.718, double_array.at(1));
-}
-
-TEST_F(TestNodeWithInitialValues, multiple_undeclared_initial_values_allowed) {
-  auto context = rclcpp::contexts::default_context::get_global_default_context();
-  const std::vector<std::string> arguments = {};
-  const std::vector<rclcpp::Parameter> initial_values = {
-    rclcpp::Parameter("foo", true),
-    rclcpp::Parameter("bar", "hello world"),
-    rclcpp::Parameter("baz", std::vector<double>{3.14, 2.718})
-  };
-  const bool use_global_arguments = false;
-  const bool use_intra_process = false;
-  const bool start_param_services = true;
-  const bool allow_undeclared_params = true;
-  auto node = rclcpp::Node::make_shared(
-    "node_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process,
-    start_param_services, allow_undeclared_params);
-
-  auto list_params_result = node->list_parameters({}, 0);
-  EXPECT_TRUE(node->get_parameter("foo").get_value<bool>());
-  EXPECT_STREQ("hello world", node->get_parameter("bar").get_value<std::string>().c_str());
-  std::vector<double> double_array = node->get_parameter("baz").get_value<std::vector<double>>();
-  ASSERT_EQ(2u, double_array.size());
-  EXPECT_DOUBLE_EQ(3.14, double_array.at(0));
-  EXPECT_DOUBLE_EQ(2.718, double_array.at(1));
-}

rclcpp/test/test_pub_sub_option_interface.cpp

@@ -0,0 +1,70 @@
+// 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 <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "test_msgs/msg/empty.hpp"
+
+class TestPubSubOptionAPI : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+};
+
+TEST_F(TestPubSubOptionAPI, check_for_ambiguous) {
+  rclcpp::PublisherOptions pub_options;
+  rclcpp::SubscriptionOptions sub_options;
+
+  auto topic_only_pub = node->create_publisher<test_msgs::msg::Empty>(
+    "topic_only");
+  auto topic_depth_pub = node->create_publisher<test_msgs::msg::Empty>(
+    "topic_depth",
+    10);
+  auto all_options_pub = node->create_publisher<test_msgs::msg::Empty>(
+    "topic_options",
+    10,
+    pub_options);
+
+  auto topic_only_sub = node->create_subscription<test_msgs::msg::Empty>(
+    "topic_only",
+    [](std::shared_ptr<test_msgs::msg::Empty>) {});
+  auto topic_depth_sub = node->create_subscription<test_msgs::msg::Empty>(
+    "topic_depth",
+    [](std::shared_ptr<test_msgs::msg::Empty>) {},
+    10);
+  auto all_options_sub = node->create_subscription<test_msgs::msg::Empty>(
+    "topic_options",
+    [](std::shared_ptr<test_msgs::msg::Empty>) {},
+    10,
+    sub_options);
+}

rclcpp/test/test_publisher.cpp

@@ -30,9 +30,9 @@ protected:
     rclcpp::init(0, nullptr);
   }
 
-  void SetUp()
+  void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
   {
-    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    node = std::make_shared<rclcpp::Node>("my_node", "/ns", node_options);
   }
 
   void TearDown()
@@ -43,10 +43,41 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
+class TestPublisherSub : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    subnode = node->create_sub_node("sub_ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr subnode;
+};
+
+static constexpr rmw_qos_profile_t invalid_qos_profile()
+{
+  rmw_qos_profile_t profile = rmw_qos_profile_default;
+  profile.depth = 1;
+  profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+  return profile;
+}
+
 /*
    Testing publisher construction and destruction.
  */
 TEST_F(TestPublisher, construction_and_destruction) {
+  initialize();
   using rcl_interfaces::msg::IntraProcessMessage;
   {
     auto publisher = node->create_publisher<IntraProcessMessage>("topic");
@@ -58,3 +89,41 @@ TEST_F(TestPublisher, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidTopicNameError);
   }
 }
+
+/*
+   Testing publisher with intraprocess enabled and invalid QoS
+ */
+TEST_F(TestPublisher, intraprocess_with_invalid_qos) {
+  initialize(rclcpp::NodeOptions().use_intra_process_comms(true));
+  rmw_qos_profile_t qos = invalid_qos_profile();
+  using rcl_interfaces::msg::IntraProcessMessage;
+  {
+    ASSERT_THROW(
+      {auto publisher = node->create_publisher<IntraProcessMessage>("topic", qos);},
+      rclcpp::exceptions::InvalidParametersException);
+  }
+}
+
+/*
+   Testing publisher construction and destruction for subnodes.
+ */
+TEST_F(TestPublisherSub, construction_and_destruction) {
+  using rcl_interfaces::msg::IntraProcessMessage;
+  {
+    auto publisher = subnode->create_publisher<IntraProcessMessage>("topic");
+
+    EXPECT_STREQ(publisher->get_topic_name(), "/ns/sub_ns/topic");
+  }
+
+  {
+    auto publisher = subnode->create_publisher<IntraProcessMessage>("/topic");
+
+    EXPECT_STREQ(publisher->get_topic_name(), "/topic");
+  }
+
+  {
+    ASSERT_THROW({
+      auto publisher = subnode->create_publisher<IntraProcessMessage>("invalid_topic?");
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+}

rclcpp/test/test_publisher_subscription_count_api.cpp

@@ -0,0 +1,178 @@
+// 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 <iostream>
+#include <string>
+#include <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcl_interfaces/msg/intra_process_message.hpp"
+
+using rcl_interfaces::msg::IntraProcessMessage;
+
+/**
+ * Parameterized test.
+ * The first param are the NodeOptions used to create the nodes.
+ * The second param are the expect intraprocess count results.
+ */
+struct TestParameters
+{
+  rclcpp::NodeOptions node_options[2];
+  uint64_t intraprocess_count_results[2];
+  std::string description;
+};
+
+std::ostream & operator<<(std::ostream & out, const TestParameters & params)
+{
+  out << params.description;
+  return out;
+}
+
+class TestPublisherSubscriptionCount : public ::testing::TestWithParam<TestParameters>
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+protected:
+  void SetUp() {}
+
+  void TearDown() {}
+
+  static std::chrono::milliseconds offset;
+};
+
+std::chrono::milliseconds TestPublisherSubscriptionCount::offset = std::chrono::milliseconds(2000);
+
+void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg)
+{
+  (void)msg;
+}
+
+TEST_P(TestPublisherSubscriptionCount, increasing_and_decreasing_counts)
+{
+  TestParameters parameters = GetParam();
+  rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>(
+    "my_node",
+    "/ns",
+    parameters.node_options[0]);
+  auto publisher = node->create_publisher<IntraProcessMessage>("/topic");
+
+  EXPECT_EQ(publisher->get_subscription_count(), 0u);
+  EXPECT_EQ(publisher->get_intra_process_subscription_count(), 0u);
+  {
+    auto sub = node->create_subscription<IntraProcessMessage>("/topic", &OnMessage);
+    rclcpp::sleep_for(offset);
+    EXPECT_EQ(publisher->get_subscription_count(), 1u);
+    EXPECT_EQ(
+      publisher->get_intra_process_subscription_count(),
+      parameters.intraprocess_count_results[0]);
+    {
+      rclcpp::Node::SharedPtr another_node = std::make_shared<rclcpp::Node>(
+        "another_node",
+        "/ns",
+        parameters.node_options[1]);
+      auto another_sub =
+        another_node->create_subscription<IntraProcessMessage>("/topic", &OnMessage);
+
+      rclcpp::sleep_for(offset);
+      EXPECT_EQ(publisher->get_subscription_count(), 2u);
+      EXPECT_EQ(
+        publisher->get_intra_process_subscription_count(),
+        parameters.intraprocess_count_results[1]);
+    }
+    rclcpp::sleep_for(offset);
+    EXPECT_EQ(publisher->get_subscription_count(), 1u);
+    EXPECT_EQ(
+      publisher->get_intra_process_subscription_count(),
+      parameters.intraprocess_count_results[0]);
+  }
+  /**
+    * Counts should be zero here, as all are subscriptions are out of scope.
+    * Subscriptions count checking is always preceeded with an sleep, as random failures had been
+    * detected without it. */
+  rclcpp::sleep_for(offset);
+  EXPECT_EQ(publisher->get_subscription_count(), 0u);
+  EXPECT_EQ(publisher->get_intra_process_subscription_count(), 0u);
+}
+
+auto get_new_context()
+{
+  auto context = rclcpp::Context::make_shared();
+  context->init(0, nullptr);
+  return context;
+}
+
+TestParameters parameters[] = {
+  /*
+     Testing publisher subscription count api and internal process subscription count.
+     Two subscriptions in the same topic, both using intraprocess comm.
+   */
+  {
+    {
+      rclcpp::NodeOptions().use_intra_process_comms(true),
+      rclcpp::NodeOptions().use_intra_process_comms(true)
+    },
+    {1u, 2u},
+    "two_subscriptions_intraprocess_comm"
+  },
+  /*
+     Testing publisher subscription count api and internal process subscription count.
+     Two subscriptions, one using intra-process comm and the other not using it.
+   */
+  {
+    {
+      rclcpp::NodeOptions().use_intra_process_comms(true),
+      rclcpp::NodeOptions().use_intra_process_comms(false)
+    },
+    {1u, 1u},
+    "two_subscriptions_one_intraprocess_one_not"
+  },
+  /*
+     Testing publisher subscription count api and internal process subscription count.
+     Two contexts, both using intra-process.
+   */
+  {
+    {
+      rclcpp::NodeOptions().use_intra_process_comms(true),
+      rclcpp::NodeOptions().context(get_new_context()).use_intra_process_comms(true)
+    },
+    {1u, 1u},
+    "two_subscriptions_in_two_contexts_with_intraprocess_comm"
+  },
+  /*
+     Testing publisher subscription count api and internal process subscription count.
+     Two contexts, both of them not using intra-process comm.
+   */
+  {
+    {
+      rclcpp::NodeOptions().use_intra_process_comms(false),
+      rclcpp::NodeOptions().context(get_new_context()).use_intra_process_comms(false)
+    },
+    {0u, 0u},
+    "two_subscriptions_in_two_contexts_without_intraprocess_comm"
+  }
+};
+
+INSTANTIATE_TEST_CASE_P(
+  TestWithDifferentNodeOptions, TestPublisherSubscriptionCount,
+  ::testing::ValuesIn(parameters),
+  ::testing::PrintToStringParamName());

rclcpp/test/test_service.cpp

@@ -43,6 +43,28 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
+class TestServiceSub : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    subnode = node->create_sub_node("sub_ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr subnode;
+};
+
 /*
    Testing service construction and destruction.
  */
@@ -61,3 +83,23 @@ TEST_F(TestService, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }
+
+/*
+   Testing service construction and destruction for subnodes.
+ */
+TEST_F(TestServiceSub, construction_and_destruction) {
+  using rcl_interfaces::srv::ListParameters;
+  auto callback =
+    [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) {
+    };
+  {
+    auto service = subnode->create_service<ListParameters>("service", callback);
+    EXPECT_STREQ(service->get_service_name(), "/ns/sub_ns/service");
+  }
+
+  {
+    ASSERT_THROW({
+      auto service = node->create_service<ListParameters>("invalid_service?", callback);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}

rclcpp/test/test_subscription.cpp

@@ -49,6 +49,34 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
+class TestSubscriptionSub : public ::testing::Test
+{
+public:
+  void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg)
+  {
+    (void)msg;
+  }
+
+protected:
+  static void SetUpTestCase()
+  {
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("test_subscription", "/ns");
+    subnode = node->create_sub_node("sub_ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr subnode;
+};
+
 class SubscriptionClassNodeInheritance : public rclcpp::Node
 {
 public:
@@ -107,6 +135,38 @@ TEST_F(TestSubscription, construction_and_destruction) {
   }
 }
 
+/*
+   Testing subscription construction and destruction for subnodes.
+ */
+TEST_F(TestSubscriptionSub, construction_and_destruction) {
+  using rcl_interfaces::msg::IntraProcessMessage;
+  auto callback = [](const IntraProcessMessage::SharedPtr msg) {
+      (void)msg;
+    };
+  {
+    auto sub = subnode->create_subscription<IntraProcessMessage>("topic", callback);
+    EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic");
+  }
+
+  {
+    auto sub = subnode->create_subscription<IntraProcessMessage>("/topic", callback);
+    EXPECT_STREQ(sub->get_topic_name(), "/topic");
+  }
+
+  {
+    auto sub = subnode->create_subscription<IntraProcessMessage>("~/topic", callback);
+    std::string expected_topic_name =
+      std::string(node->get_namespace()) + "/" + node->get_name() + "/topic";
+    EXPECT_STREQ(sub->get_topic_name(), expected_topic_name.c_str());
+  }
+
+  {
+    ASSERT_THROW({
+      auto sub = node->create_subscription<IntraProcessMessage>("invalid_topic?", callback);
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+}
+
 /*
    Testing subscriptions using std::bind.
  */

rclcpp/test/test_subscription_publisher_count_api.cpp

@@ -0,0 +1,123 @@
+// 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 <iostream>
+#include <string>
+#include <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcl_interfaces/msg/intra_process_message.hpp"
+
+using rcl_interfaces::msg::IntraProcessMessage;
+
+struct TestParameters
+{
+  rclcpp::NodeOptions node_options;
+  std::string description;
+};
+
+std::ostream & operator<<(std::ostream & out, const TestParameters & params)
+{
+  out << params.description;
+  return out;
+}
+
+class TestSubscriptionPublisherCount : public ::testing::TestWithParam<TestParameters>
+{
+public:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+protected:
+  void SetUp() {}
+  void TearDown() {}
+  static std::chrono::milliseconds offset;
+};
+
+std::chrono::milliseconds TestSubscriptionPublisherCount::offset = std::chrono::milliseconds(2000);
+
+void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg)
+{
+  (void)msg;
+}
+
+TEST_P(TestSubscriptionPublisherCount, increasing_and_decreasing_counts)
+{
+  rclcpp::NodeOptions node_options = GetParam().node_options;
+  rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>(
+    "my_node",
+    "/ns",
+    node_options);
+  auto subscription = node->create_subscription<IntraProcessMessage>("/topic", &OnMessage);
+
+  EXPECT_EQ(subscription->get_publisher_count(), 0u);
+  {
+    auto pub = node->create_publisher<IntraProcessMessage>("/topic");
+    rclcpp::sleep_for(offset);
+    EXPECT_EQ(subscription->get_publisher_count(), 1u);
+    {
+      rclcpp::Node::SharedPtr another_node = std::make_shared<rclcpp::Node>(
+        "another_node",
+        "/ns",
+        node_options);
+      auto another_pub =
+        another_node->create_publisher<IntraProcessMessage>("/topic");
+
+      rclcpp::sleep_for(offset);
+      EXPECT_EQ(subscription->get_publisher_count(), 2u);
+    }
+    rclcpp::sleep_for(offset);
+    EXPECT_EQ(subscription->get_publisher_count(), 1u);
+  }
+  rclcpp::sleep_for(offset);
+  EXPECT_EQ(subscription->get_publisher_count(), 0u);
+}
+
+auto get_new_context()
+{
+  auto context = rclcpp::Context::make_shared();
+  context->init(0, nullptr);
+  return context;
+}
+
+TestParameters parameters[] = {
+  /*
+     Testing subscription publisher count api.
+     One context.
+   */
+  {
+    rclcpp::NodeOptions(),
+    "one_context_test"
+  },
+  /*
+     Testing subscription publisher count api.
+     Two contexts.
+   */
+  {
+    rclcpp::NodeOptions().context(get_new_context()),
+    "two_contexts_test"
+  }
+};
+
+INSTANTIATE_TEST_CASE_P(
+  TestWithDifferentNodeOptions,
+  TestSubscriptionPublisherCount,
+  testing::ValuesIn(parameters),
+  testing::PrintToStringParamName());

rclcpp/test/test_time_source.cpp

@@ -55,16 +55,83 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
-void trigger_clock_changes(
-  rclcpp::Node::SharedPtr node)
+void spin_until_time(
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Node::SharedPtr node,
+  std::chrono::nanoseconds end_time,
+  bool expect_time_update)
 {
-  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
-      rmw_qos_profile_default);
+  // Call spin_once on the node until either:
+  // 1) We see the ros_clock's simulated time change to the expected end_time
+  // -or-
+  // 2) 1 second has elapsed in the real world
+  // If 'expect_time_update' is True, and we timed out waiting for simulated time to
+  // update, we'll have the test fail
 
   rclcpp::executors::SingleThreadedExecutor executor;
   executor.add_node(node);
 
-  rclcpp::WallRate loop_rate(50);
+  auto start = std::chrono::system_clock::now();
+  while (std::chrono::system_clock::now() < (start + 1s)) {
+    if (!rclcpp::ok()) {
+      break;  // Break for ctrl-c
+    }
+
+    executor.spin_once(10ms);
+
+    if (clock->now().nanoseconds() == end_time.count()) {
+      return;
+    }
+  }
+
+  if (expect_time_update) {
+    // If we were expecting ROS clock->now to be updated and we didn't take the early return from
+    // the loop up above, that's a failure
+    ASSERT_TRUE(false) << "Timed out waiting for ROS time to update";
+  }
+}
+
+void spin_until_ros_time_updated(
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Node::SharedPtr node,
+  rclcpp::ParameterValue value)
+{
+  // Similar to above:  Call spin_once until we see the clock's ros_time_is_active method
+  // match the ParameterValue
+  // Unlike spin_until_time, there aren't any test cases where we don't expect the value to
+  // update.  In the event that the ParameterValue is not set, we'll pump messages for a full second
+  // but we don't cause the test to fail
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+
+  auto start = std::chrono::system_clock::now();
+  while (std::chrono::system_clock::now() < (start + 1s)) {
+    if (!rclcpp::ok()) {
+      break;  // Break for ctrl-c
+    }
+
+    executor.spin_once(10ms);
+
+    // In the case where we didn't intend to change the parameter, we'll still pump
+    if (value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      continue;
+    }
+
+    if (clock->ros_time_is_active() == value.get<bool>()) {
+      return;
+    }
+  }
+}
+
+void trigger_clock_changes(
+  rclcpp::Node::SharedPtr node,
+  std::shared_ptr<rclcpp::Clock> clock,
+  bool expect_time_update = true)
+{
+  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
+      rmw_qos_profile_default);
+
   for (int i = 0; i < 5; ++i) {
     if (!rclcpp::ok()) {
       break;  // Break for ctrl-c
@@ -73,12 +140,23 @@ void trigger_clock_changes(
     msg->clock.sec = i;
     msg->clock.nanosec = 1000;
     clock_pub->publish(msg);
-    executor.spin_once(1000000ns);
-    loop_rate.sleep();
+
+    // workaround.  Long-term, there can be a more elegant fix where we hook a future up
+    // to a clock change callback and spin until future complete, but that's an upstream
+    // change
+    spin_until_time(
+      clock,
+      node,
+      std::chrono::seconds(i) + std::chrono::nanoseconds(1000),
+      expect_time_update
+    );
   }
 }
 
-void set_use_sim_time_parameter(rclcpp::Node::SharedPtr node, rclcpp::ParameterValue value)
+void set_use_sim_time_parameter(
+  rclcpp::Node::SharedPtr node,
+  rclcpp::ParameterValue value,
+  rclcpp::Clock::SharedPtr clock)
 {
   auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
 
@@ -90,10 +168,12 @@ void set_use_sim_time_parameter(rclcpp::Node::SharedPtr node, rclcpp::ParameterV
   for (auto & result : set_parameters_results) {
     EXPECT_TRUE(result.successful);
   }
-  // SyncParametersClient returns when parameters have been set on the node_parameters interface,
-  // but it doesn't mean the on_parameter_event subscription in TimeSource has been called.
-  // Spin some to handle that subscription.
-  rclcpp::spin_some(node);
+
+  // Same as above - workaround for a little bit of asynchronus behavior.  The sim_time paramater
+  // is set synchronously, but the way the ros clock gets notified involves a pub/sub that happens
+  // AFTER the synchronous notification that the parameter was set.  This may also get fixed
+  // upstream
+  spin_until_ros_time_updated(clock, node, value);
 }
 
 TEST_F(TestTimeSource, detachUnattached) {
@@ -154,7 +234,7 @@ TEST_F(TestTimeSource, ROS_time_valid_sim_time) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  node->set_parameter_if_not_set("use_sim_time", true);
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
   ts.attachNode(node);
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
@@ -169,17 +249,17 @@ TEST_F(TestTimeSource, clock) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  trigger_clock_changes(node);
+  trigger_clock_changes(node, ros_clock, false);
 
   // Even now that we've recieved a message, ROS time should still not be active since the
   // parameter has not been explicitly set.
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
   // Activate ROS time.
-  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
-  trigger_clock_changes(node);
+  trigger_clock_changes(node, ros_clock);
 
   auto t_out = ros_clock->now();
 
@@ -236,7 +316,10 @@ TEST_F(TestTimeSource, callbacks) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  trigger_clock_changes(node);
+  // Last arg below is 'expect_time_update'  Since ros_time is not active yet, we don't expect
+  // the simulated time to be updated by trigger_clock_changes.  The method will pump messages
+  // anyway, but won't fail the test when the simulated time doesn't update
+  trigger_clock_changes(node, ros_clock, false);
   auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
   auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
 
@@ -245,10 +328,10 @@ TEST_F(TestTimeSource, callbacks) {
   EXPECT_EQ(0, cbo.last_postcallback_id_);
 
   // Activate ROS time.
-  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
-  trigger_clock_changes(node);
+  trigger_clock_changes(node, ros_clock);
 
   auto t_out = ros_clock->now();
 
@@ -266,7 +349,7 @@ TEST_F(TestTimeSource, callbacks) {
     std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2),
     jump_threshold);
 
-  trigger_clock_changes(node);
+  trigger_clock_changes(node, ros_clock);
 
   EXPECT_EQ(2, cbo.last_precallback_id_);
   EXPECT_EQ(2, cbo.last_postcallback_id_);
@@ -285,7 +368,7 @@ TEST_F(TestTimeSource, callbacks) {
     std::function<void(rcl_time_jump_t)>(),
     jump_threshold);
 
-  trigger_clock_changes(node);
+  trigger_clock_changes(node, ros_clock);
   EXPECT_EQ(3, cbo.last_precallback_id_);
   EXPECT_EQ(2, cbo.last_postcallback_id_);
 
@@ -295,7 +378,7 @@ TEST_F(TestTimeSource, callbacks) {
     std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 4),
     jump_threshold);
 
-  trigger_clock_changes(node);
+  trigger_clock_changes(node, ros_clock);
   EXPECT_EQ(3, cbo.last_precallback_id_);
   EXPECT_EQ(4, cbo.last_postcallback_id_);
 }
@@ -330,10 +413,10 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
   EXPECT_EQ(0, cbo.last_postcallback_id_);
 
   // Activate ROS time.
-  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
-  trigger_clock_changes(node);
+  trigger_clock_changes(node, ros_clock);
 
   auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
   auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
@@ -357,7 +440,7 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
   // Remove the last callback in the vector
   callback_handler2.reset();
 
-  trigger_clock_changes(node);
+  trigger_clock_changes(node, ros_clock);
 
   EXPECT_EQ(2, cbo.last_precallback_id_);
   EXPECT_EQ(2, cbo.last_postcallback_id_);
@@ -371,7 +454,6 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
   EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
 }
 
-
 TEST_F(TestTimeSource, parameter_activation) {
   rclcpp::TimeSource ts(node);
   auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
@@ -380,27 +462,19 @@ TEST_F(TestTimeSource, parameter_activation) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
-  set_use_sim_time_parameter(
-    node, rclcpp::ParameterValue());
-  EXPECT_TRUE(ros_clock->ros_time_is_active());
-
-  set_use_sim_time_parameter(node, rclcpp::ParameterValue(false));
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
-
-  set_use_sim_time_parameter(
-    node, rclcpp::ParameterValue());
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(false), ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
   // If the use_sim_time parameter is not explicitly set to True, this clock's use of sim time
   // should not be affected by the presence of a clock publisher.
-  trigger_clock_changes(node);
+  trigger_clock_changes(node, ros_clock, false);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
-  set_use_sim_time_parameter(node, rclcpp::ParameterValue(false));
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(false), ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
-  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 }
 
@@ -425,7 +499,7 @@ TEST_F(TestTimeSource, no_pre_jump_callback) {
   ts.attachClock(ros_clock);
 
   // Activate ROS time
-  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
   ASSERT_TRUE(ros_clock->ros_time_is_active());
 
   EXPECT_EQ(0, cbo.last_precallback_id_);

rclcpp/test/test_timer.cpp

@@ -0,0 +1,152 @@
+// 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 <exception>
+#include <memory>
+
+#include "rcl/timer.h"
+
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using namespace std::chrono_literals;
+
+/// Timer testing bring up and teardown
+class TestTimer : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+    has_timer_run.store(false);
+    cancel_timer.store(false);
+
+    test_node = std::make_shared<rclcpp::Node>("test_timer_node");
+
+    timer = test_node->create_wall_timer(100ms,
+        [this]() -> void
+        {
+          this->has_timer_run.store(true);
+
+          if (this->cancel_timer.load()) {
+            this->timer->cancel();
+          }
+          // prevent any tests running timer from blocking
+          this->executor->cancel();
+        }
+    );
+
+    executor->add_node(test_node);
+    // don't start spinning, let the test dictate when
+  }
+
+  void TearDown() override
+  {
+    timer.reset();
+    test_node.reset();
+    executor.reset();
+    rclcpp::shutdown();
+  }
+
+  // set to true if the timer callback executed, false otherwise
+  std::atomic<bool> has_timer_run;
+  // flag used to cancel the timer in the timer callback. If true cancel the timer, otherwise
+  // cancel the executor (preventing any tests from blocking)
+  std::atomic<bool> cancel_timer;
+  rclcpp::Node::SharedPtr test_node;
+  std::shared_ptr<rclcpp::TimerBase> timer;
+  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor;
+};
+
+/// check if initial states are set as expected
+void test_initial_conditions(
+  std::shared_ptr<rclcpp::TimerBase> & timer,
+  std::atomic<bool> & has_timer_run)
+{
+  ASSERT_FALSE(timer->is_canceled());
+  ASSERT_FALSE(has_timer_run.load());
+}
+
+/// Simple test
+TEST_F(TestTimer, test_simple_cancel)
+{
+  // expect clean state, don't run otherwise
+  test_initial_conditions(timer, has_timer_run);
+
+  // cancel
+  timer->cancel();
+  EXPECT_TRUE(timer->is_canceled());
+
+  EXPECT_FALSE(has_timer_run.load());
+}
+
+/// Test state when using reset
+TEST_F(TestTimer, test_is_canceled_reset)
+{
+  // expect clean state, don't run otherwise
+  test_initial_conditions(timer, has_timer_run);
+
+  // reset shouldn't affect state (not canceled yet)
+  timer->reset();
+  EXPECT_FALSE(timer->is_canceled());
+
+  // cancel after reset
+  timer->cancel();
+  EXPECT_TRUE(timer->is_canceled());
+
+  // reset and cancel
+  timer->reset();
+  EXPECT_FALSE(timer->is_canceled());
+  timer->cancel();
+  EXPECT_TRUE(timer->is_canceled());
+
+  EXPECT_FALSE(has_timer_run.load());
+}
+
+/// Run and check state, cancel the executor
+TEST_F(TestTimer, test_run_cancel_executor)
+{
+  // expect clean state, don't run otherwise
+  test_initial_conditions(timer, has_timer_run);
+
+  // run the timer (once, this forces an executor cancel so spin won't block)
+  // but the timer was not explicitly cancelled
+  executor->spin();
+  EXPECT_TRUE(has_timer_run.load());
+
+  // force a timer cancel
+  EXPECT_FALSE(timer->is_canceled());
+  timer->cancel();
+  EXPECT_TRUE(timer->is_canceled());
+}
+
+/// Run and check state, cancel the timer
+TEST_F(TestTimer, test_run_cancel_timer)
+{
+  // expect clean state, don't run otherwise
+  test_initial_conditions(timer, has_timer_run);
+
+  // force a timer cancellation
+  cancel_timer.store(true);
+  // run the timer (once, this forces an executor cancel so spin won't block)
+  executor->spin();
+  EXPECT_TRUE(has_timer_run.load());
+  EXPECT_TRUE(timer->is_canceled());
+}

rclcpp_action/CHANGELOG.rst

@@ -2,6 +2,23 @@
 Changelog for package rclcpp_action
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+
+0.7.1 (2019-04-26)
+------------------
+* Added optional callbacks to action client for goal, response, and result. (`#701 <https://github.com/ros2/rclcpp/issues/701>`_)
+* Added overload for node interfaces. (`#700 <https://github.com/ros2/rclcpp/issues/700>`_)
+* Renamed action state transitions. (`#677 <https://github.com/ros2/rclcpp/issues/677>`_)
+* Contributors: Jacob Perron, Karsten Knese
+
+0.7.0 (2019-04-14)
+------------------
+* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 <https://github.com/ros2/rclcpp/issues/648>`_)
+* Added documentation to rclcpp_action. (`#650 <https://github.com/ros2/rclcpp/pull/650>`_)
+* Updated to use separated action types. (`#601 <https://github.com/ros2/rclcpp/issues/601>`_)
+* Updated to wait for action server before sending goal. (`#637 <https://github.com/ros2/rclcpp/issues/637>`_)
+* Refactored server goal handle's try_canceling() function. (`#603 <https://github.com/ros2/rclcpp/issues/603>`_)
+* Contributors: Emerson Knapp, Jacob Perron, Michel Hidalgo, Shane Loretz
+
 0.6.2 (2018-12-13)
 ------------------
 

rclcpp_action/Doxyfile

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

rclcpp_action/include/rclcpp_action/client.hpp

@@ -66,10 +66,10 @@ public:
   action_server_is_ready() const;
 
   /// Wait for action_server_is_ready() to become true, or until the given timeout is reached.
-  template<typename RatioT = std::milli>
+  template<typename RepT = int64_t, typename RatioT = std::milli>
   bool
   wait_for_action_server(
-    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+    std::chrono::duration<RepT, RatioT> timeout = std::chrono::duration<RepT, RatioT>(-1))
   {
     return wait_for_action_server_nanoseconds(
       std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
@@ -148,7 +148,7 @@ protected:
   /// \internal
   RCLCPP_ACTION_PUBLIC
   virtual
-  GoalID
+  GoalUUID
   generate_goal_id();
 
   /// \internal
@@ -245,7 +245,7 @@ private:
 /**
  * This class creates an action client.
  *
- * Create an instance of this server using `rclcpp_action::create_client()`.
+ * To create an instance of an action client use `rclcpp_action::create_client()`.
  *
  * Internally, this class is responsible for:
  *  - coverting between the C++ action type and generic types for `rclcpp_action::ClientBase`, and
@@ -260,11 +260,59 @@ public:
   using Goal = typename ActionT::Goal;
   using Feedback = typename ActionT::Feedback;
   using GoalHandle = ClientGoalHandle<ActionT>;
-  using Result = typename GoalHandle::Result;
-  using FeedbackCallback = typename ClientGoalHandle<ActionT>::FeedbackCallback;
-  using CancelRequest = typename ActionT::CancelGoalService::Request;
-  using CancelResponse = typename ActionT::CancelGoalService::Response;
+  using WrappedResult = typename GoalHandle::WrappedResult;
+  using GoalResponseCallback =
+    std::function<void (std::shared_future<typename GoalHandle::SharedPtr>)>;
+  using FeedbackCallback = typename GoalHandle::FeedbackCallback;
+  using ResultCallback = typename GoalHandle::ResultCallback;
+  using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
+  using CancelResponse = typename ActionT::Impl::CancelGoalService::Response;
+  using CancelOneCallback = std::function<void (typename GoalHandle::SharedPtr, bool)>;
+  using CancelMultipleCallback = std::function<void (typename CancelResponse::SharedPtr)>;
 
+  /// Options for sending a goal.
+  /**
+   * This struct is used to pass parameters to the function `async_send_goal`.
+   */
+  struct SendGoalOptions
+  {
+    SendGoalOptions()
+    : goal_response_callback(nullptr),
+      feedback_callback(nullptr),
+      result_callback(nullptr)
+    {
+    }
+
+    /// Function called when the goal is accepted or rejected.
+    /**
+     * Takes a single argument that is a future to a goal handle shared pointer.
+     * If the goal is accepted, then the pointer points to a valid goal handle.
+     * If the goal is rejected, then pointer has the value `nullptr`.
+     * If an error occurs while waiting for the goal response an exception will be thrown
+     * when calling `future::get()`.
+     * Possible exceptions include `rclcpp::RCLError` and `rclcpp::RCLBadAlloc`.
+     */
+    GoalResponseCallback goal_response_callback;
+
+    /// Function called whenever feedback is received for the goal.
+    FeedbackCallback feedback_callback;
+
+    /// Function called when the result for the goal is received.
+    ResultCallback result_callback;
+  };
+
+  /// Construct an action client.
+  /**
+   * This constructs an action client, but it will not work until it has been added to a node.
+   * Use `rclcpp_action::create_client()` to both construct and add to a node.
+   *
+   * \param[in] node_base A pointer to the base interface of a node.
+   * \param[in] node_graph A pointer to an interface that allows getting graph information about
+   *   a node.
+   * \param[in] node_logging A pointer to an interface that allows getting a node's logger.
+   * \param[in] action_name The action name.
+   * \param[in] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`.
+   */
   Client(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
@@ -279,58 +327,88 @@ public:
   {
   }
 
+  /// Send an action goal and asynchronously get the result.
+  /**
+   * If the goal is accepted by an action server, the returned future is set to a `ClientGoalHandle`.
+   * If the goal is rejected by an action server, then the future is set to a `nullptr`.
+   *
+   * The goal handle is used to monitor the status of the goal and get the final result.
+   *
+   * \param[in] goal The goal request.
+   * \param[in] options Options for sending the goal request. Contains references to callbacks for
+   *   the goal response (accepted/rejected), feedback, and the final result.
+   * \return A future that completes when the goal has been accepted or rejected.
+   *   If the goal is rejected, then the result will be a `nullptr`.
+   */
   std::shared_future<typename GoalHandle::SharedPtr>
-  async_send_goal(
-    const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false)
+  async_send_goal(const Goal & goal, const SendGoalOptions & options = SendGoalOptions())
   {
     // Put promise in the heap to move it around.
     auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
     std::shared_future<typename GoalHandle::SharedPtr> future(promise->get_future());
-    using GoalRequest = typename ActionT::GoalRequestService::Request;
-    // auto goal_request = std::make_shared<GoalRequest>();
-    // goal_request->goal_id = this->generate_goal_id();
-    // goal_request->goal = goal;
-    auto goal_request = std::make_shared<GoalRequest>(goal);
-    goal_request->action_goal_id.uuid = this->generate_goal_id();
+    using GoalRequest = typename ActionT::Impl::SendGoalService::Request;
+    auto goal_request = std::make_shared<GoalRequest>();
+    goal_request->goal_id.uuid = this->generate_goal_id();
+    goal_request->goal = goal;
     this->send_goal_request(
       std::static_pointer_cast<void>(goal_request),
-      [this, goal_request, callback, ignore_result, promise](
-        std::shared_ptr<void> response) mutable
+      [this, goal_request, options, promise, future](std::shared_ptr<void> response) mutable
       {
-        using GoalResponse = typename ActionT::GoalRequestService::Response;
+        using GoalResponse = typename ActionT::Impl::SendGoalService::Response;
         auto goal_response = std::static_pointer_cast<GoalResponse>(response);
         if (!goal_response->accepted) {
           promise->set_value(nullptr);
+          if (options.goal_response_callback) {
+            options.goal_response_callback(future);
+          }
           return;
         }
         GoalInfo goal_info;
-        // goal_info.goal_id = goal_request->goal_id;
-        goal_info.goal_id.uuid = goal_request->action_goal_id.uuid;
+        goal_info.goal_id.uuid = goal_request->goal_id.uuid;
         goal_info.stamp = goal_response->stamp;
         // Do not use std::make_shared as friendship cannot be forwarded.
-        std::shared_ptr<GoalHandle> goal_handle(new GoalHandle(goal_info, callback));
-        if (!ignore_result) {
+        std::shared_ptr<GoalHandle> goal_handle(
+          new GoalHandle(goal_info, options.feedback_callback, options.result_callback));
+        if (options.result_callback) {
           try {
             this->make_result_aware(goal_handle);
           } catch (...) {
             promise->set_exception(std::current_exception());
+            options.goal_response_callback(future);
             return;
           }
         }
         std::lock_guard<std::mutex> guard(goal_handles_mutex_);
         goal_handles_[goal_handle->get_goal_id()] = goal_handle;
         promise->set_value(goal_handle);
+        if (options.goal_response_callback) {
+          options.goal_response_callback(future);
+        }
       });
     return future;
   }
 
-  std::shared_future<Result>
-  async_get_result(typename GoalHandle::SharedPtr goal_handle)
+  /// Asynchronously get the result for an active goal.
+  /**
+   * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal
+   *   state.
+   * \param[in] goal_handle The goal handle for which to get the result.
+   * \param[in] result_callback Optional callback that is called when the result is received.
+   * \return A future that is set to the goal result when the goal is finished.
+   */
+  std::shared_future<WrappedResult>
+  async_get_result(
+    typename GoalHandle::SharedPtr goal_handle,
+    ResultCallback result_callback = nullptr)
   {
     std::lock_guard<std::mutex> lock(goal_handles_mutex_);
     if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
       throw exceptions::UnknownGoalHandleError();
     }
+    if (result_callback) {
+      // This will override any previously registered callback
+      goal_handle->set_result_callback(result_callback);
+    }
     // If the user chose to ignore the result before, then ask the server for the result now.
     if (!goal_handle->is_result_aware()) {
       this->make_result_aware(goal_handle);
@@ -338,8 +416,20 @@ public:
     return goal_handle->async_result();
   }
 
+  /// Asynchronously request a goal be canceled.
+  /**
+   * \throws exceptions::UnknownGoalHandleError If the goal is unknown or already reached a
+   *   terminal state.
+   * \param[in] goal_handle The goal handle requesting to be canceled.
+   * \param[in] cancel_callback Optional callback that is called when the response is received.
+   *   The callback function takes two parameters: a shared pointer to the goal handle and a bool
+   *   indicating if the action server accepted the cancel request or not.
+   * \return A future whose result indicates whether or not the cancel request was accepted.
+   */
   std::shared_future<bool>
-  async_cancel_goal(typename GoalHandle::SharedPtr goal_handle)
+  async_cancel_goal(
+    typename GoalHandle::SharedPtr goal_handle,
+    CancelOneCallback cancel_callback = nullptr)
   {
     std::lock_guard<std::mutex> lock(goal_handles_mutex_);
     if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
@@ -353,7 +443,7 @@ public:
     cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id();
     this->send_cancel_request(
       std::static_pointer_cast<void>(cancel_request),
-      [goal_handle, promise](std::shared_ptr<void> response) mutable
+      [goal_handle, cancel_callback, promise](std::shared_ptr<void> response) mutable
       {
         auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
         bool goal_canceled = false;
@@ -363,23 +453,49 @@ public:
           goal_canceled = (canceled_goal_info.goal_id.uuid == goal_handle->get_goal_id());
         }
         promise->set_value(goal_canceled);
+        if (cancel_callback) {
+          cancel_callback(goal_handle, goal_canceled);
+        }
       });
     return future;
   }
 
+  /// Asynchronously request for all goals to be canceled.
+  /**
+   * \param[in] cancel_callback Optional callback that is called when the response is received.
+   *   The callback takes one parameter: a shared pointer to the CancelResponse message.
+   * \return A future to a CancelResponse message that is set when the request has been
+   * acknowledged by an action server.
+   * See
+   * <a href="https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/srv/CancelGoal.srv">
+   * action_msgs/CancelGoal.srv</a>.
+   */
   std::shared_future<typename CancelResponse::SharedPtr>
-  async_cancel_all_goals()
+  async_cancel_all_goals(CancelMultipleCallback cancel_callback = nullptr)
   {
     auto cancel_request = std::make_shared<CancelRequest>();
     // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
     std::fill(
       cancel_request->goal_info.goal_id.uuid.begin(),
       cancel_request->goal_info.goal_id.uuid.end(), 0u);
-    return async_cancel(cancel_request);
+    return async_cancel(cancel_request, cancel_callback);
   }
 
+  /// Asynchronously request all goals at or before a specified time be canceled.
+  /**
+   * \param[in] stamp The timestamp for the cancel goal request.
+   * \param[in] cancel_callback Optional callback that is called when the response is received.
+   *   The callback takes one parameter: a shared pointer to the CancelResponse message.
+   * \return A future to a CancelResponse message that is set when the request has been
+   * acknowledged by an action server.
+   * See
+   * <a href="https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/srv/CancelGoal.srv">
+   * action_msgs/CancelGoal.srv</a>.
+   */
   std::shared_future<typename CancelResponse::SharedPtr>
-  async_cancel_goals_before(const rclcpp::Time & stamp)
+  async_cancel_goals_before(
+    const rclcpp::Time & stamp,
+    CancelMultipleCallback cancel_callback = nullptr)
   {
     auto cancel_request = std::make_shared<CancelRequest>();
     // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
@@ -387,7 +503,7 @@ public:
       cancel_request->goal_info.goal_id.uuid.begin(),
       cancel_request->goal_info.goal_id.uuid.end(), 0u);
     cancel_request->goal_info.stamp = stamp;
-    return async_cancel(cancel_request);
+    return async_cancel(cancel_request, cancel_callback);
   }
 
   virtual
@@ -406,7 +522,7 @@ private:
   std::shared_ptr<void>
   create_goal_response() const override
   {
-    using GoalResponse = typename ActionT::GoalRequestService::Response;
+    using GoalResponse = typename ActionT::Impl::SendGoalService::Response;
     return std::shared_ptr<void>(new GoalResponse());
   }
 
@@ -414,7 +530,7 @@ private:
   std::shared_ptr<void>
   create_result_response() const override
   {
-    using GoalResultResponse = typename ActionT::GoalResultService::Response;
+    using GoalResultResponse = typename ActionT::Impl::GetResultService::Response;
     return std::shared_ptr<void>(new GoalResultResponse());
   }
 
@@ -429,9 +545,8 @@ private:
   std::shared_ptr<void>
   create_feedback_message() const override
   {
-    // using FeedbackMessage = typename ActionT::FeedbackMessage;
-    // return std::shared_ptr<void>(new FeedbackMessage());
-    return std::shared_ptr<void>(new Feedback());
+    using FeedbackMessage = typename ActionT::Impl::FeedbackMessage;
+    return std::shared_ptr<void>(new FeedbackMessage());
   }
 
   /// \internal
@@ -439,13 +554,10 @@ private:
   handle_feedback_message(std::shared_ptr<void> message) override
   {
     std::lock_guard<std::mutex> guard(goal_handles_mutex_);
-    // using FeedbackMessage = typename ActionT::FeedbackMessage;
-    // typename FeedbackMessage::SharedPtr feedback_message =
-    //   std::static_pointer_cast<FeedbackMessage>(message);
-    typename Feedback::SharedPtr feedback_message =
-      std::static_pointer_cast<Feedback>(message);
-    // const GoalID & goal_id = feedback_message->goal_id;
-    const GoalID & goal_id = feedback_message->action_goal_id.uuid;
+    using FeedbackMessage = typename ActionT::Impl::FeedbackMessage;
+    typename FeedbackMessage::SharedPtr feedback_message =
+      std::static_pointer_cast<FeedbackMessage>(message);
+    const GoalUUID & goal_id = feedback_message->goal_id.uuid;
     if (goal_handles_.count(goal_id) == 0) {
       RCLCPP_DEBUG(
         this->get_logger(),
@@ -453,15 +565,16 @@ private:
       return;
     }
     typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
-    // goal_handle->call_feedback_callback(goal_handle, feedback_message->feedback);
-    goal_handle->call_feedback_callback(goal_handle, feedback_message);
+    auto feedback = std::make_shared<Feedback>();
+    *feedback = feedback_message->feedback;
+    goal_handle->call_feedback_callback(goal_handle, feedback);
   }
 
   /// \internal
   std::shared_ptr<void>
   create_status_message() const override
   {
-    using GoalStatusMessage = typename ActionT::GoalStatusMessage;
+    using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage;
     return std::shared_ptr<void>(new GoalStatusMessage());
   }
 
@@ -470,11 +583,10 @@ private:
   handle_status_message(std::shared_ptr<void> message) override
   {
     std::lock_guard<std::mutex> guard(goal_handles_mutex_);
-    using GoalStatusMessage = typename ActionT::GoalStatusMessage;
+    using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage;
     auto status_message = std::static_pointer_cast<GoalStatusMessage>(message);
     for (const GoalStatus & status : status_message->status_list) {
-      // const GoalID & goal_id = status.goal_info.goal_id;
-      const GoalID & goal_id = status.goal_info.goal_id.uuid;
+      const GoalUUID & goal_id = status.goal_info.goal_id.uuid;
       if (goal_handles_.count(goal_id) == 0) {
         RCLCPP_DEBUG(
           this->get_logger(),
@@ -498,21 +610,22 @@ private:
   void
   make_result_aware(typename GoalHandle::SharedPtr goal_handle)
   {
-    using GoalResultRequest = typename ActionT::GoalResultService::Request;
+    using GoalResultRequest = typename ActionT::Impl::GetResultService::Request;
     auto goal_result_request = std::make_shared<GoalResultRequest>();
-    // goal_result_request.goal_id = goal_handle->get_goal_id();
-    goal_result_request->action_goal_id.uuid = goal_handle->get_goal_id();
+    goal_result_request->goal_id.uuid = goal_handle->get_goal_id();
     this->send_result_request(
       std::static_pointer_cast<void>(goal_result_request),
       [goal_handle, this](std::shared_ptr<void> response) mutable
       {
         // Wrap the response in a struct with the fields a user cares about
-        Result result;
-        using GoalResultResponse = typename ActionT::GoalResultService::Response;
-        result.response = std::static_pointer_cast<GoalResultResponse>(response);
-        result.goal_id = goal_handle->get_goal_id();
-        result.code = static_cast<ResultCode>(result.response->action_status);
-        goal_handle->set_result(result);
+        WrappedResult wrapped_result;
+        using GoalResultResponse = typename ActionT::Impl::GetResultService::Response;
+        auto result_response = std::static_pointer_cast<GoalResultResponse>(response);
+        wrapped_result.result = std::make_shared<typename ActionT::Result>();
+        *wrapped_result.result = result_response->result;
+        wrapped_result.goal_id = goal_handle->get_goal_id();
+        wrapped_result.code = static_cast<ResultCode>(result_response->status);
+        goal_handle->set_result(wrapped_result);
         std::lock_guard<std::mutex> lock(goal_handles_mutex_);
         goal_handles_.erase(goal_handle->get_goal_id());
       });
@@ -521,22 +634,27 @@ private:
 
   /// \internal
   std::shared_future<typename CancelResponse::SharedPtr>
-  async_cancel(typename CancelRequest::SharedPtr cancel_request)
+  async_cancel(
+    typename CancelRequest::SharedPtr cancel_request,
+    CancelMultipleCallback cancel_callback = nullptr)
   {
     // Put promise in the heap to move it around.
     auto promise = std::make_shared<std::promise<typename CancelResponse::SharedPtr>>();
     std::shared_future<typename CancelResponse::SharedPtr> future(promise->get_future());
     this->send_cancel_request(
       std::static_pointer_cast<void>(cancel_request),
-      [promise](std::shared_ptr<void> response) mutable
+      [cancel_callback, promise](std::shared_ptr<void> response) mutable
       {
         auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
         promise->set_value(cancel_response);
+        if (cancel_callback) {
+          cancel_callback(cancel_response);
+        }
       });
     return future;
   }
 
-  std::map<GoalID, typename GoalHandle::SharedPtr> goal_handles_;
+  std::map<GoalUUID, typename GoalHandle::SharedPtr> goal_handles_;
   std::mutex goal_handles_mutex_;
 };
 }  // namespace rclcpp_action

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -45,6 +45,15 @@ enum class ResultCode : int8_t
 template<typename ActionT>
 class Client;
 
+/// Class for interacting with goals sent from action clients.
+/**
+ * Use this class to check the status of a goal as well as get the result.
+ *
+ * This class is not meant to be created by a user, instead it is created when a goal has been
+ * accepted.
+ * A `Client` will create an instance and return it to the user (via a future) after calling
+ * `Client::async_send_goal`.
+ */
 template<typename ActionT>
 class ClientGoalHandle
 {
@@ -52,38 +61,56 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle)
 
   // A wrapper that defines the result of an action
-  typedef struct Result
+  typedef struct WrappedResult
   {
     /// The unique identifier of the goal
-    GoalID goal_id;
+    GoalUUID goal_id;
     /// A status to indicate if the goal was canceled, aborted, or suceeded
     ResultCode code;
     /// User defined fields sent back with an action
-    typename ActionT::Result::SharedPtr response;
-  } Result;
+    typename ActionT::Result::SharedPtr result;
+  } WrappedResult;
 
   using Feedback = typename ActionT::Feedback;
+  using Result = typename ActionT::Result;
   using FeedbackCallback =
     std::function<void (
-        typename ClientGoalHandle<ActionT>::SharedPtr, const std::shared_ptr<const Feedback>)>;
+        typename ClientGoalHandle<ActionT>::SharedPtr,
+        const std::shared_ptr<const Feedback>)>;
+  using ResultCallback = std::function<void (const WrappedResult & result)>;
 
   virtual ~ClientGoalHandle();
 
-  const GoalID &
+  /// Get the unique ID for the goal.
+  const GoalUUID &
   get_goal_id() const;
 
+  /// Get the time when the goal was accepted.
   rclcpp::Time
   get_goal_stamp() const;
 
-  std::shared_future<Result>
+  /// Get a future to the goal result.
+  /**
+   * This method should not be called if the `ignore_result` flag was set when
+   * sending the original goal request (see Client::async_send_goal).
+   *
+   * `is_result_aware()` can be used to check if it is safe to call this method.
+   *
+   * \throws exceptions::UnawareGoalHandleError If the the goal handle is unaware of the result.
+   * \return A future to the result.
+   */
+  std::shared_future<WrappedResult>
   async_result();
 
+  /// Get the goal status code.
   int8_t
   get_status();
 
+  /// Check if an action client has subscribed to feedback for the goal.
   bool
   is_feedback_aware();
 
+  /// Check if an action client has requested the result for the goal.
   bool
   is_result_aware();
 
@@ -91,11 +118,17 @@ private:
   // The templated Client creates goal handles
   friend Client<ActionT>;
 
-  ClientGoalHandle(const GoalInfo & info, FeedbackCallback callback);
+  ClientGoalHandle(
+    const GoalInfo & info,
+    FeedbackCallback feedback_callback,
+    ResultCallback result_callback);
 
   void
   set_feedback_callback(FeedbackCallback callback);
 
+  void
+  set_result_callback(ResultCallback callback);
+
   void
   call_feedback_callback(
     typename ClientGoalHandle<ActionT>::SharedPtr shared_this,
@@ -108,7 +141,7 @@ private:
   set_status(int8_t status);
 
   void
-  set_result(const Result & result);
+  set_result(const WrappedResult & wrapped_result);
 
   void
   invalidate();
@@ -116,10 +149,11 @@ private:
   GoalInfo info_;
 
   bool is_result_aware_{false};
-  std::promise<Result> result_promise_;
-  std::shared_future<Result> result_future_;
+  std::promise<WrappedResult> result_promise_;
+  std::shared_future<WrappedResult> result_future_;
 
   FeedbackCallback feedback_callback_{nullptr};
+  ResultCallback result_callback_{nullptr};
   int8_t status_{GoalStatus::STATUS_ACCEPTED};
 
   std::mutex handle_mutex_;

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -28,8 +28,11 @@ namespace rclcpp_action
 
 template<typename ActionT>
 ClientGoalHandle<ActionT>::ClientGoalHandle(
-  const GoalInfo & info, FeedbackCallback callback)
-: info_(info), result_future_(result_promise_.get_future()), feedback_callback_(callback)
+  const GoalInfo & info, FeedbackCallback feedback_callback, ResultCallback result_callback)
+: info_(info),
+  result_future_(result_promise_.get_future()),
+  feedback_callback_(feedback_callback),
+  result_callback_(result_callback)
 {
 }
 
@@ -39,10 +42,9 @@ ClientGoalHandle<ActionT>::~ClientGoalHandle()
 }
 
 template<typename ActionT>
-const GoalID &
+const GoalUUID &
 ClientGoalHandle<ActionT>::get_goal_id() const
 {
-  // return info_.goal_id;
   return info_.goal_id.uuid;
 }
 
@@ -54,7 +56,7 @@ ClientGoalHandle<ActionT>::get_goal_stamp() const
 }
 
 template<typename ActionT>
-std::shared_future<typename ClientGoalHandle<ActionT>::Result>
+std::shared_future<typename ClientGoalHandle<ActionT>::WrappedResult>
 ClientGoalHandle<ActionT>::async_result()
 {
   std::lock_guard<std::mutex> guard(handle_mutex_);
@@ -66,11 +68,14 @@ ClientGoalHandle<ActionT>::async_result()
 
 template<typename ActionT>
 void
-ClientGoalHandle<ActionT>::set_result(const Result & result)
+ClientGoalHandle<ActionT>::set_result(const WrappedResult & wrapped_result)
 {
   std::lock_guard<std::mutex> guard(handle_mutex_);
-  status_ = static_cast<int8_t>(result.code);
-  result_promise_.set_value(result);
+  status_ = static_cast<int8_t>(wrapped_result.code);
+  result_promise_.set_value(wrapped_result);
+  if (result_callback_) {
+    result_callback_(wrapped_result);
+  }
 }
 
 template<typename ActionT>
@@ -81,6 +86,14 @@ ClientGoalHandle<ActionT>::set_feedback_callback(FeedbackCallback callback)
   feedback_callback_ = callback;
 }
 
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_result_callback(ResultCallback callback)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  result_callback_ = callback;
+}
+
 template<typename ActionT>
 int8_t
 ClientGoalHandle<ActionT>::get_status()

rclcpp_action/include/rclcpp_action/create_client.hpp

@@ -25,15 +25,31 @@
 
 namespace rclcpp_action
 {
+/// Create an action client.
+/**
+ * This function is equivalent to \sa create_client()` however is using the individual
+ * node interfaces to create the client.
+ *
+ * \param node_base_interface[in] The node base interface of the corresponding node.
+ * \param node_graph_interface[in] The node graph interface of the corresponding node.
+ * \param node_logging_interface[in] The node logging interface of the corresponding node.
+ * \param node_waitables_interface[in] The node waitables interface of the corresponding node.
+ * \param[in] name The action name.
+ * \param[in] group The action client will be added to this callback group.
+ *   If `nullptr`, then the action client is added to the default callback group.
+ */
 template<typename ActionT>
 typename Client<ActionT>::SharedPtr
 create_client(
-  rclcpp::Node::SharedPtr node,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface,
   const std::string & name,
   rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
 {
   std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
-    node->get_node_waitables_interface();
+    node_waitables_interface;
   std::weak_ptr<rclcpp::callback_group::CallbackGroup> weak_group = group;
   bool group_is_null = (nullptr == group.get());
 
@@ -64,15 +80,38 @@ create_client(
 
   std::shared_ptr<Client<ActionT>> action_client(
     new Client<ActionT>(
-      node->get_node_base_interface(),
-      node->get_node_graph_interface(),
-      node->get_node_logging_interface(),
+      node_base_interface,
+      node_graph_interface,
+      node_logging_interface,
       name),
     deleter);
 
-  node->get_node_waitables_interface()->add_waitable(action_client, group);
+  node_waitables_interface->add_waitable(action_client, group);
   return action_client;
 }
+
+/// Create an action client.
+/**
+ * \param[in] node The action client will be added to this node.
+ * \param[in] name The action name.
+ * \param[in] group The action client will be added to this callback group.
+ *   If `nullptr`, then the action client is added to the default callback group.
+ */
+template<typename ActionT>
+typename Client<ActionT>::SharedPtr
+create_client(
+  rclcpp::Node::SharedPtr node,
+  const std::string & name,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
+{
+  return create_client<ActionT>(
+    node->get_node_base_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_logging_interface(),
+    node->get_node_waitables_interface(),
+    name,
+    group);
+}
 }  // namespace rclcpp_action
 
 #endif  // RCLCPP_ACTION__CREATE_CLIENT_HPP_

rclcpp_action/include/rclcpp_action/create_server.hpp

@@ -31,10 +31,35 @@
 
 namespace rclcpp_action
 {
+/// Create an action server.
+/**
+ * All provided callback functions must be non-blocking.
+ * This function is equivalent to \sa create_server()` however is using the individual
+ * node interfaces to create the server.
+ *
+ * \sa Server::Server() for more information.
+ *
+ * \param node_base_interface[in] The node base interface of the corresponding node.
+ * \param node_clock_interface[in] The node clock interface of the corresponding node.
+ * \param node_logging_interface[in] The node logging interface of the corresponding node.
+ * \param node_waitables_interface[in] The node waitables interface of the corresponding node.
+ * \param name[in] The action name.
+ * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected.
+ * \param[in] handle_cancel A callback that decides if a goal should be attempted to be canceled.
+ *  The return from this callback only indicates if the server will try to cancel a goal.
+ *  It does not indicate if the goal was actually canceled.
+ * \param[in] handle_accepted A callback that is called to give the user a handle to the goal.
+ * \param[in] options options to pass to the underlying `rcl_action_server_t`.
+ * \param group[in] The action server will be added to this callback group.
+ *   If `nullptr`, then the action server is added to the default callback group.
+ */
 template<typename ActionT>
 typename Server<ActionT>::SharedPtr
 create_server(
-  rclcpp::Node::SharedPtr node,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface,
   const std::string & name,
   typename Server<ActionT>::GoalCallback handle_goal,
   typename Server<ActionT>::CancelCallback handle_cancel,
@@ -43,7 +68,7 @@ create_server(
   rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
 {
   std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
-    node->get_node_waitables_interface();
+    node_waitables_interface;
   std::weak_ptr<rclcpp::callback_group::CallbackGroup> weak_group = group;
   bool group_is_null = (nullptr == group.get());
 
@@ -73,17 +98,58 @@ create_server(
     };
 
   std::shared_ptr<Server<ActionT>> action_server(new Server<ActionT>(
-      node->get_node_base_interface(),
-      node->get_node_clock_interface(),
-      node->get_node_logging_interface(),
+      node_base_interface,
+      node_clock_interface,
+      node_logging_interface,
       name,
       options,
       handle_goal,
       handle_cancel,
       handle_accepted), deleter);
 
-  node->get_node_waitables_interface()->add_waitable(action_server, group);
+  node_waitables_interface->add_waitable(action_server, group);
   return action_server;
 }
+
+/// Create an action server.
+/**
+ * All provided callback functions must be non-blocking.
+ *
+ * \sa Server::Server() for more information.
+ *
+ * \param node[in] The action server will be added to this node.
+ * \param name[in] The action name.
+ * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected.
+ * \param[in] handle_cancel A callback that decides if a goal should be attempted to be canceled.
+ *  The return from this callback only indicates if the server will try to cancel a goal.
+ *  It does not indicate if the goal was actually canceled.
+ * \param[in] handle_accepted A callback that is called to give the user a handle to the goal.
+ * \param[in] options options to pass to the underlying `rcl_action_server_t`.
+ * \param group[in] The action server will be added to this callback group.
+ *   If `nullptr`, then the action server is added to the default callback group.
+ */
+template<typename ActionT>
+typename Server<ActionT>::SharedPtr
+create_server(
+  rclcpp::Node::SharedPtr node,
+  const std::string & name,
+  typename Server<ActionT>::GoalCallback handle_goal,
+  typename Server<ActionT>::CancelCallback handle_cancel,
+  typename Server<ActionT>::AcceptedCallback handle_accepted,
+  const rcl_action_server_options_t & options = rcl_action_server_get_default_options(),
+  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
+{
+  return create_server<ActionT>(
+    node->get_node_base_interface(),
+    node->get_node_clock_interface(),
+    node->get_node_logging_interface(),
+    node->get_node_waitables_interface(),
+    name,
+    handle_goal,
+    handle_cancel,
+    handle_accepted,
+    options,
+    group);
+}
 }  // namespace rclcpp_action
 #endif  // RCLCPP_ACTION__CREATE_SERVER_HPP_

rclcpp_action/include/rclcpp_action/rclcpp_action.hpp

@@ -17,9 +17,14 @@
  * `rclcpp_action` provides the canonical C++ API for interacting with ROS Actions.
  * It consists of these main components:
  *
- * - TODO(jacobperron): Finish docs
  * - Action Client
+ *   - rclcpp_action/client.hpp
+ *   - rclcpp_action/create_client.hpp
+ *   - rclcpp_action/client_goal_handle.hpp
  * - Action Server
+ *   - rclcpp_action/server.hpp
+ *   - rclcpp_action/create_server.hpp
+ *   - rclcpp_action/server_goal_handle.hpp
  */
 
 #ifndef RCLCPP_ACTION__RCLCPP_ACTION_HPP_

rclcpp_action/include/rclcpp_action/server.hpp

@@ -63,7 +63,7 @@ enum class CancelResponse : int8_t
 /// \internal
 /**
  * This class should not be used directly by users writing an action server.
- * Instead users should use `rclcpp_action::Server<>`.
+ * Instead users should use `rclcpp_action::Server`.
  *
  * Internally, this class is responsible for interfacing with the `rcl_action` API.
  */
@@ -146,7 +146,7 @@ protected:
   RCLCPP_ACTION_PUBLIC
   virtual
   std::pair<GoalResponse, std::shared_ptr<void>>
-  call_handle_goal_callback(GoalID &, std::shared_ptr<void> request) = 0;
+  call_handle_goal_callback(GoalUUID &, std::shared_ptr<void> request) = 0;
 
   // ServerBase will determine which goal ids are being cancelled, and then call this function for
   // each goal id.
@@ -155,13 +155,13 @@ protected:
   RCLCPP_ACTION_PUBLIC
   virtual
   CancelResponse
-  call_handle_cancel_callback(const GoalID & uuid) = 0;
+  call_handle_cancel_callback(const GoalUUID & uuid) = 0;
 
   /// Given a goal request message, return the UUID contained within.
   /// \internal
   RCLCPP_ACTION_PUBLIC
   virtual
-  GoalID
+  GoalUUID
   get_goal_id_from_goal_request(void * message) = 0;
 
   /// Create an empty goal request message so it can be taken from a lower layer.
@@ -178,13 +178,13 @@ protected:
   void
   call_goal_accepted_callback(
     std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
-    GoalID uuid, std::shared_ptr<void> goal_request_message) = 0;
+    GoalUUID uuid, std::shared_ptr<void> goal_request_message) = 0;
 
   /// Given a result request message, return the UUID contained within.
   /// \internal
   RCLCPP_ACTION_PUBLIC
   virtual
-  GoalID
+  GoalUUID
   get_goal_id_from_result_request(void * message) = 0;
 
   /// Create an empty goal request message so it can be taken from a lower layer.
@@ -214,7 +214,7 @@ protected:
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg);
+  publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_msg);
 
   /// \internal
   RCLCPP_ACTION_PUBLIC
@@ -272,7 +272,7 @@ public:
 
   /// Signature of a callback that accepts or rejects goal requests.
   using GoalCallback = std::function<GoalResponse(
-        const GoalID &, std::shared_ptr<const typename ActionT::Goal>)>;
+        const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>;
   /// Signature of a callback that accepts or rejects requests to cancel a goal.
   using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
   /// Signature of a callback that is used to notify when the goal has been accepted.
@@ -288,7 +288,7 @@ public:
    *  - one to accept or reject requests to cancel a goal,
    *  - one to receive a goal handle after a goal has been accepted.
    * All callbacks must be non-blocking.
-   * The result of a goal should be set using methods on `rclcpp_action::ServerGoalHandle<>`.
+   * The result of a goal should be set using methods on `rclcpp_action::ServerGoalHandle`.
    *
    * \param[in] node_base a pointer to the base interface of a node.
    * \param[in] node_clock a pointer to an interface that allows getting a node's clock.
@@ -335,16 +335,14 @@ protected:
 
   /// \internal
   std::pair<GoalResponse, std::shared_ptr<void>>
-  call_handle_goal_callback(GoalID & uuid, std::shared_ptr<void> message) override
+  call_handle_goal_callback(GoalUUID & uuid, std::shared_ptr<void> message) override
   {
-    // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type
-    static_assert(
-      std::is_same<typename ActionT::Goal, typename ActionT::GoalRequestService::Request>::value,
-      "Assuming user fields were merged with goal request fields");
-    GoalResponse user_response = handle_goal_(
-      uuid, std::static_pointer_cast<typename ActionT::Goal>(message));
+    auto request = std::static_pointer_cast<
+      typename ActionT::Impl::SendGoalService::Request>(message);
+    auto goal = std::shared_ptr<typename ActionT::Goal>(request, &request->goal);
+    GoalResponse user_response = handle_goal_(uuid, goal);
 
-    auto ros_response = std::make_shared<typename ActionT::GoalRequestService::Response>();
+    auto ros_response = std::make_shared<typename ActionT::Impl::SendGoalService::Response>();
     ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response ||
       GoalResponse::ACCEPT_AND_DEFER == user_response;
     return std::make_pair(user_response, ros_response);
@@ -352,7 +350,7 @@ protected:
 
   /// \internal
   CancelResponse
-  call_handle_cancel_callback(const GoalID & uuid) override
+  call_handle_cancel_callback(const GoalUUID & uuid) override
   {
     std::lock_guard<std::mutex> lock(goal_handles_mutex_);
     CancelResponse resp = CancelResponse::REJECT;
@@ -362,7 +360,7 @@ protected:
       if (goal_handle) {
         resp = handle_cancel_(goal_handle);
         if (CancelResponse::ACCEPT == resp) {
-          goal_handle->_set_canceling();
+          goal_handle->_cancel_goal();
         }
       }
     }
@@ -373,13 +371,13 @@ protected:
   void
   call_goal_accepted_callback(
     std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
-    GoalID uuid, std::shared_ptr<void> goal_request_message) override
+    GoalUUID uuid, std::shared_ptr<void> goal_request_message) override
   {
     std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
     std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();
 
-    std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state =
-      [weak_this](const GoalID & uuid, std::shared_ptr<void> result_message)
+    std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state =
+      [weak_this](const GoalUUID & uuid, std::shared_ptr<void> result_message)
       {
         std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
         if (!shared_this) {
@@ -396,8 +394,8 @@ protected:
         shared_this->goal_handles_.erase(uuid);
       };
 
-    std::function<void(const GoalID &)> on_executing =
-      [weak_this](const GoalID & uuid)
+    std::function<void(const GoalUUID &)> on_executing =
+      [weak_this](const GoalUUID & uuid)
       {
         std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
         if (!shared_this) {
@@ -408,8 +406,8 @@ protected:
         shared_this->publish_status();
       };
 
-    std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback =
-      [weak_this](std::shared_ptr<typename ActionT::Feedback> feedback_msg)
+    std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback =
+      [weak_this](std::shared_ptr<typename ActionT::Impl::FeedbackMessage> feedback_msg)
       {
         std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
         if (!shared_this) {
@@ -418,11 +416,12 @@ protected:
         shared_this->publish_feedback(std::static_pointer_cast<void>(feedback_msg));
       };
 
+    auto request = std::static_pointer_cast<
+      const typename ActionT::Impl::SendGoalService::Request>(goal_request_message);
+    auto goal = std::shared_ptr<const typename ActionT::Goal>(request, &request->goal);
     goal_handle.reset(
       new ServerGoalHandle<ActionT>(
-        rcl_goal_handle, uuid,
-        std::static_pointer_cast<const typename ActionT::Goal>(goal_request_message),
-        on_terminal_state, on_executing, publish_feedback));
+        rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback));
     {
       std::lock_guard<std::mutex> lock(goal_handles_mutex_);
       goal_handles_[uuid] = goal_handle;
@@ -431,41 +430,41 @@ protected:
   }
 
   /// \internal
-  GoalID
+  GoalUUID
   get_goal_id_from_goal_request(void * message) override
   {
     return
-      static_cast<typename ActionT::GoalRequestService::Request *>(message)->action_goal_id.uuid;
+      static_cast<typename ActionT::Impl::SendGoalService::Request *>(message)->goal_id.uuid;
   }
 
   /// \internal
   std::shared_ptr<void>
   create_goal_request() override
   {
-    return std::shared_ptr<void>(new typename ActionT::GoalRequestService::Request());
+    return std::shared_ptr<void>(new typename ActionT::Impl::SendGoalService::Request());
   }
 
   /// \internal
-  GoalID
+  GoalUUID
   get_goal_id_from_result_request(void * message) override
   {
     return
-      static_cast<typename ActionT::GoalResultService::Request *>(message)->action_goal_id.uuid;
+      static_cast<typename ActionT::Impl::GetResultService::Request *>(message)->goal_id.uuid;
   }
 
   /// \internal
   std::shared_ptr<void>
   create_result_request() override
   {
-    return std::shared_ptr<void>(new typename ActionT::GoalResultService::Request());
+    return std::shared_ptr<void>(new typename ActionT::Impl::GetResultService::Request());
   }
 
   /// \internal
   std::shared_ptr<void>
   create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
   {
-    auto result = std::make_shared<typename ActionT::GoalResultService::Response>();
-    result->action_status = status;
+    auto result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
+    result->status = status;
     return std::static_pointer_cast<void>(result);
   }
 
@@ -480,7 +479,7 @@ private:
   using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>;
   /// A map of goal id to goal handle weak pointers.
   /// This is used to provide a goal handle to handle_cancel.
-  std::unordered_map<GoalID, GoalHandleWeakPtr> goal_handles_;
+  std::unordered_map<GoalUUID, GoalHandleWeakPtr> goal_handles_;
   std::mutex goal_handles_mutex_;
 };
 }  // namespace rclcpp_action

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp

@@ -80,27 +80,27 @@ protected:
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  _set_aborted();
+  _abort();
 
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  _set_succeeded();
+  _succeed();
 
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  _set_canceling();
+  _cancel_goal();
 
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  _set_canceled();
+  _canceled();
 
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  _set_executing();
+  _execute();
 
   /// Transition the goal to canceled state if it never reached a terminal state.
   /// \internal
@@ -123,10 +123,10 @@ class Server;
 /// Class to interact with goals on a server.
 /**
  * Use this class to check the status of a goal as well as set the result.
+ *
  * This class is not meant to be created by a user, instead it is created when a goal has been
  * accepted.
- * The class `rclcpp_action::Server<>` will create an instance and give it to the user in their
- * `handle_accepted` callback.
+ * A `Server` will create an instance and give it to the user in their `handle_accepted` callback.
  *
  * Internally, this class is responsible for coverting between the C++ action type and generic
  * types for `rclcpp_action::ServerGoalHandleBase`.
@@ -138,52 +138,60 @@ public:
   /// Send an update about the progress of a goal.
   /**
    * This must only be called when the goal is executing.
-   * If execution of a goal is deferred then `ServerGoalHandle<>::set_executing()` must be called
+   * If execution of a goal is deferred then `ServerGoalHandle::set_executing()` must be called
    * first.
-   * `std::runtime_error` is raised if the goal is in any state besides executing.
+   *
+   * \throws std::runtime_error If the goal is in any state besides executing.
    *
    * \param[in] feedback_msg the message to publish to clients.
    */
   void
   publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
   {
-    feedback_msg->action_goal_id.uuid = uuid_;
-    publish_feedback_(feedback_msg);
+    auto feedback_message = std::make_shared<typename ActionT::Impl::FeedbackMessage>();
+    feedback_message->goal_id.uuid = uuid_;
+    feedback_message->feedback = *feedback_msg;
+    publish_feedback_(feedback_message);
   }
 
-  // TODO(sloretz) which exception is raised?
   /// Indicate that a goal could not be reached and has been aborted.
   /**
    * Only call this if the goal was executing but cannot be completed.
    * This is a terminal state, no more methods should be called on a goal handle after this is
    * called.
-   * An exception is raised if the goal is in any state besides executing.
+   *
+   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
    *
    * \param[in] result_msg the final result to send to clients.
    */
   void
-  set_aborted(typename ActionT::Result::SharedPtr result_msg)
+  abort(typename ActionT::Result::SharedPtr result_msg)
   {
-    _set_aborted();
-    result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
-    on_terminal_state_(uuid_, result_msg);
+    _abort();
+    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
+    response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
+    response->result = *result_msg;
+    on_terminal_state_(uuid_, response);
   }
 
-  /// Indicate that a goal has been reached.
+  /// Indicate that a goal has succeeded.
   /**
    * Only call this if the goal is executing and has reached the desired final state.
    * This is a terminal state, no more methods should be called on a goal handle after this is
    * called.
-   * An exception is raised if the goal is in any state besides executing.
+   *
+   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
    *
    * \param[in] result_msg the final result to send to clients.
    */
   void
-  set_succeeded(typename ActionT::Result::SharedPtr result_msg)
+  succeed(typename ActionT::Result::SharedPtr result_msg)
   {
-    _set_succeeded();
-    result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
-    on_terminal_state_(uuid_, result_msg);
+    _succeed();
+    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
+    response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
+    response->result = *result_msg;
+    on_terminal_state_(uuid_, response);
   }
 
   /// Indicate that a goal has been canceled.
@@ -191,31 +199,35 @@ public:
    * Only call this if the goal is executing or pending, but has been canceled.
    * This is a terminal state, no more methods should be called on a goal handle after this is
    * called.
-   * An exception is raised if the goal is in any state besides executing or pending.
+   *
+   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
    *
    * \param[in] result_msg the final result to send to clients.
    */
   void
-  set_canceled(typename ActionT::Result::SharedPtr result_msg)
+  canceled(typename ActionT::Result::SharedPtr result_msg)
   {
-    _set_canceled();
-    result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
-    on_terminal_state_(uuid_, result_msg);
+    _canceled();
+    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
+    response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
+    response->result = *result_msg;
+    on_terminal_state_(uuid_, response);
   }
 
   /// Indicate that the server is starting to execute a goal.
   /**
    * Only call this if the goal is pending.
-   * An exception is raised if the goal is in any state besides pending.
+   *
+   * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing.
    */
   void
-  set_executing()
+  execute()
   {
-    _set_executing();
+    _execute();
     on_executing_(uuid_);
   }
 
-  /// Get the original request message describing the goal.
+  /// Get the user provided message describing the goal.
   const std::shared_ptr<const typename ActionT::Goal>
   get_goal() const
   {
@@ -223,7 +235,7 @@ public:
   }
 
   /// Get the unique identifier of the goal
-  const GoalID &
+  const GoalUUID &
   get_goal_id() const
   {
     return uuid_;
@@ -233,8 +245,8 @@ public:
   {
     // Cancel goal if handle was allowed to destruct without reaching a terminal state
     if (try_canceling()) {
-      auto null_result = std::make_shared<typename ActionT::Result>();
-      null_result->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
+      auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
+      null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
       on_terminal_state_(uuid_, null_result);
     }
   }
@@ -243,11 +255,11 @@ protected:
   /// \internal
   ServerGoalHandle(
     std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
-    GoalID uuid,
+    GoalUUID uuid,
     std::shared_ptr<const typename ActionT::Goal> goal,
-    std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state,
-    std::function<void(const GoalID &)> on_executing,
-    std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback
+    std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state,
+    std::function<void(const GoalUUID &)> on_executing,
+    std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback
   )
   : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid),
     on_terminal_state_(on_terminal_state), on_executing_(on_executing),
@@ -255,17 +267,17 @@ protected:
   {
   }
 
-  /// The original request message describing the goal.
+  /// The user provided message describing the goal.
   const std::shared_ptr<const typename ActionT::Goal> goal_;
 
   /// A unique id for the goal request.
-  const GoalID uuid_;
+  const GoalUUID uuid_;
 
   friend Server<ActionT>;
 
-  std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state_;
-  std::function<void(const GoalID &)> on_executing_;
-  std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback_;
+  std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;
+  std::function<void(const GoalUUID &)> on_executing_;
+  std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;
 };
 }  // namespace rclcpp_action
 

rclcpp_action/include/rclcpp_action/types.hpp

@@ -29,34 +29,34 @@
 namespace rclcpp_action
 {
 
-using GoalID = std::array<uint8_t, UUID_SIZE>;
+using GoalUUID = std::array<uint8_t, UUID_SIZE>;
 using GoalStatus = action_msgs::msg::GoalStatus;
 using GoalInfo = action_msgs::msg::GoalInfo;
 
 /// Convert a goal id to a human readable string.
 RCLCPP_ACTION_PUBLIC
 std::string
-to_string(const GoalID & goal_id);
+to_string(const GoalUUID & goal_id);
 
 // Convert C++ GoalID to rcl_action_goal_info_t
 RCLCPP_ACTION_PUBLIC
 void
-convert(const GoalID & goal_id, rcl_action_goal_info_t * info);
+convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info);
 
 // Convert rcl_action_goal_info_t to C++ GoalID
 RCLCPP_ACTION_PUBLIC
 void
-convert(const rcl_action_goal_info_t & info, GoalID * goal_id);
+convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id);
 }  // namespace rclcpp_action
 
 namespace std
 {
 template<>
-struct less<rclcpp_action::GoalID>
+struct less<rclcpp_action::GoalUUID>
 {
   bool operator()(
-    const rclcpp_action::GoalID & lhs,
-    const rclcpp_action::GoalID & rhs) const
+    const rclcpp_action::GoalUUID & lhs,
+    const rclcpp_action::GoalUUID & rhs) const
   {
     return lhs < rhs;
   }
@@ -64,9 +64,9 @@ struct less<rclcpp_action::GoalID>
 
 /// Hash a goal id so it can be used as a key in std::unordered_map
 template<>
-struct hash<rclcpp_action::GoalID>
+struct hash<rclcpp_action::GoalUUID>
 {
-  size_t operator()(const rclcpp_action::GoalID & uuid) const noexcept
+  size_t operator()(const rclcpp_action::GoalUUID & uuid) const noexcept
   {
     // TODO(sloretz) Use someone else's hash function and cite it
     size_t result = 0;

rclcpp_action/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_action</name>
-  <version>0.6.2</version>
+  <version>0.7.1</version>
   <description>Adds action APIs for C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>

rclcpp_action/src/client.cpp

@@ -366,10 +366,10 @@ ClientBase::send_cancel_request(std::shared_ptr<void> request, ResponseCallback
   pimpl_->pending_cancel_responses[sequence_number] = callback;
 }
 
-GoalID
+GoalUUID
 ClientBase::generate_goal_id()
 {
-  GoalID goal_id;
+  GoalUUID goal_id;
   // TODO(hidmic): Do something better than this for UUID generation.
   // std::generate(
   //   goal_id.uuid.begin(), goal_id.uuid.end(),

rclcpp_action/src/server.cpp

@@ -28,7 +28,7 @@
 #include <vector>
 
 using rclcpp_action::ServerBase;
-using rclcpp_action::GoalID;
+using rclcpp_action::GoalUUID;
 
 namespace rclcpp_action
 {
@@ -62,11 +62,11 @@ public:
   bool goal_expired_ = false;
 
   // Results to be kept until the goal expires after reaching a terminal state
-  std::unordered_map<GoalID, std::shared_ptr<void>> goal_results_;
+  std::unordered_map<GoalUUID, std::shared_ptr<void>> goal_results_;
   // Requests for results are kept until a result becomes available
-  std::unordered_map<GoalID, std::vector<rmw_request_id_t>> result_requests_;
+  std::unordered_map<GoalUUID, std::vector<rmw_request_id_t>> result_requests_;
   // rcl goal handles are kept so api to send result doesn't try to access freed memory
-  std::unordered_map<GoalID, std::shared_ptr<rcl_action_goal_handle_t>> goal_handles_;
+  std::unordered_map<GoalUUID, std::shared_ptr<rcl_action_goal_handle_t>> goal_handles_;
 
   rclcpp::Logger logger_;
 };
@@ -228,7 +228,7 @@ ServerBase::execute_goal_request_received()
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 
-  GoalID uuid = get_goal_id_from_goal_request(message.get());
+  GoalUUID uuid = get_goal_id_from_goal_request(message.get());
   convert(uuid, &goal_info);
 
   // Call user's callback, getting the user's response and a ros message to send back
@@ -339,7 +339,7 @@ ServerBase::execute_cancel_request_received()
   // For each canceled goal, call cancel callback
   for (size_t i = 0; i < goals.size; ++i) {
     const rcl_action_goal_info_t & goal_info = goals.data[i];
-    GoalID uuid;
+    GoalUUID uuid;
     convert(goal_info, &uuid);
     auto response_code = call_handle_cancel_callback(uuid);
     if (CancelResponse::ACCEPT == response_code) {
@@ -388,7 +388,7 @@ ServerBase::execute_result_request_received()
   std::shared_ptr<void> result_response;
 
   // check if the goal exists
-  GoalID uuid = get_goal_id_from_result_request(result_request.get());
+  GoalUUID uuid = get_goal_id_from_result_request(result_request.get());
   rcl_action_goal_info_t goal_info;
   convert(uuid, &goal_info);
   bool goal_exists;
@@ -433,7 +433,7 @@ ServerBase::execute_check_expired_goals()
       rclcpp::exceptions::throw_from_rcl_error(ret);
     } else if (num_expired) {
       // A goal expired!
-      GoalID uuid;
+      GoalUUID uuid;
       convert(expired_goals[0], &uuid);
       RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str());
       pimpl_->goal_results_.erase(uuid);
@@ -497,7 +497,7 @@ ServerBase::publish_status()
 }
 
 void
-ServerBase::publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg)
+ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_msg)
 {
   // Check that the goal exists
   rcl_action_goal_info_t goal_info;

rclcpp_action/src/server_goal_handle.cpp

@@ -58,47 +58,47 @@ ServerGoalHandleBase::is_executing() const
 }
 
 void
-ServerGoalHandleBase::_set_aborted()
+ServerGoalHandleBase::_abort()
 {
   std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
-  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_ABORTED);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_ABORT);
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 }
 
 void
-ServerGoalHandleBase::_set_succeeded()
+ServerGoalHandleBase::_succeed()
 {
   std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
-  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_SUCCEEDED);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SUCCEED);
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 }
 
 void
-ServerGoalHandleBase::_set_canceling()
+ServerGoalHandleBase::_cancel_goal()
 {
   std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
-  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL);
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 }
 
 void
-ServerGoalHandleBase::_set_canceled()
+ServerGoalHandleBase::_canceled()
 {
   std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
-  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED);
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
 }
 
 void
-ServerGoalHandleBase::_set_executing()
+ServerGoalHandleBase::_execute()
 {
   std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
   rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE);
@@ -116,7 +116,7 @@ ServerGoalHandleBase::try_canceling() noexcept
   const bool is_cancelable = rcl_action_goal_handle_is_cancelable(rcl_handle_.get());
   if (is_cancelable) {
     // Transition to CANCELING
-    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL);
+    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL);
     if (RCL_RET_OK != ret) {
       return false;
     }
@@ -131,7 +131,7 @@ ServerGoalHandleBase::try_canceling() noexcept
 
   // If it's canceling, cancel it
   if (GOAL_STATE_CANCELING == state) {
-    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED);
+    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED);
     return RCL_RET_OK == ret;
   }
 

rclcpp_action/src/types.cpp

@@ -20,7 +20,7 @@
 namespace rclcpp_action
 {
 std::string
-to_string(const GoalID & goal_id)
+to_string(const GoalUUID & goal_id)
 {
   std::stringstream stream;
   stream << std::hex;
@@ -31,7 +31,7 @@ to_string(const GoalID & goal_id)
 }
 
 void
-convert(const GoalID & goal_id, rcl_action_goal_info_t * info)
+convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info)
 {
   for (size_t i = 0; i < 16; ++i) {
     info->goal_id.uuid[i] = goal_id[i];
@@ -39,7 +39,7 @@ convert(const GoalID & goal_id, rcl_action_goal_info_t * info)
 }
 
 void
-convert(const rcl_action_goal_info_t & info, GoalID * goal_id)
+convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id)
 {
   for (size_t i = 0; i < 16; ++i) {
     (*goal_id)[i] = info.goal_id.uuid[i];

rclcpp_action/test/test_client.cpp

@@ -24,7 +24,6 @@
 #include <rclcpp/clock.hpp>
 #include <rclcpp/exceptions.hpp>
 #include <rclcpp/executors.hpp>
-#include <rclcpp/executors/multi_threaded_executor.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -47,23 +46,25 @@
 
 using namespace std::chrono_literals;
 
+const auto WAIT_FOR_SERVER_TIMEOUT = 10s;
+
 class TestClient : public ::testing::Test
 {
 protected:
   using ActionType = test_msgs::action::Fibonacci;
   using ActionGoal = ActionType::Goal;
   using ActionGoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
-  using ActionGoalRequestService = ActionType::GoalRequestService;
+  using ActionGoalRequestService = ActionType::Impl::SendGoalService;
   using ActionGoalRequest = ActionGoalRequestService::Request;
   using ActionGoalResponse = ActionGoalRequestService::Response;
-  using ActionGoalResultService = ActionType::GoalResultService;
+  using ActionGoalResultService = ActionType::Impl::GetResultService;
   using ActionGoalResultRequest = ActionGoalResultService::Request;
   using ActionGoalResultResponse = ActionGoalResultService::Response;
-  using ActionCancelGoalService = ActionType::CancelGoalService;
-  using ActionCancelGoalRequest = ActionType::CancelGoalService::Request;
-  using ActionCancelGoalResponse = ActionType::CancelGoalService::Response;
-  using ActionStatusMessage = ActionType::GoalStatusMessage;
-  using ActionFeedbackMessage = ActionType::Feedback;
+  using ActionCancelGoalService = ActionType::Impl::CancelGoalService;
+  using ActionCancelGoalRequest = ActionType::Impl::CancelGoalService::Request;
+  using ActionCancelGoalResponse = ActionType::Impl::CancelGoalService::Response;
+  using ActionStatusMessage = ActionType::Impl::GoalStatusMessage;
+  using ActionFeedbackMessage = ActionType::Impl::FeedbackMessage;
   using ActionFeedback = ActionType::Feedback;
 
   static void SetUpTestCase()
@@ -88,9 +89,9 @@ protected:
         ActionGoalResponse::SharedPtr response)
       {
         response->stamp = clock.now();
-        response->accepted = (request->order >= 0);
+        response->accepted = (request->goal.order >= 0);
         if (response->accepted) {
-          goals[request->action_goal_id.uuid] = {request, response};
+          goals[request->goal_id.uuid] = {request, response};
         }
       });
     ASSERT_TRUE(goal_service != nullptr);
@@ -106,29 +107,30 @@ protected:
         const ActionGoalResultRequest::SharedPtr request,
         ActionGoalResultResponse::SharedPtr response)
       {
-        if (goals.count(request->action_goal_id.uuid) == 1) {
-          auto goal_request = goals[request->action_goal_id.uuid].first;
-          auto goal_response = goals[request->action_goal_id.uuid].second;
+        if (goals.count(request->goal_id.uuid) == 1) {
+          auto goal_request = goals[request->goal_id.uuid].first;
+          auto goal_response = goals[request->goal_id.uuid].second;
           ActionStatusMessage status_message;
           rclcpp_action::GoalStatus goal_status;
-          goal_status.goal_info.goal_id.uuid = goal_request->action_goal_id.uuid;
+          goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid;
           goal_status.goal_info.stamp = goal_response->stamp;
           goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING;
           status_message.status_list.push_back(goal_status);
           status_publisher->publish(status_message);
           client_executor.spin_once();
           ActionFeedbackMessage feedback_message;
-          feedback_message.action_goal_id.uuid = goal_request->action_goal_id.uuid;
-          feedback_message.sequence.push_back(0);
+          feedback_message.goal_id.uuid = goal_request->goal_id.uuid;
+          feedback_message.feedback.sequence.push_back(0);
           feedback_publisher->publish(feedback_message);
           client_executor.spin_once();
-          if (goal_request->order > 0) {
-            feedback_message.sequence.push_back(1);
+          if (goal_request->goal.order > 0) {
+            feedback_message.feedback.sequence.push_back(1);
             feedback_publisher->publish(feedback_message);
             client_executor.spin_once();
-            for (int i = 1; i < goal_request->order; ++i) {
-              feedback_message.sequence.push_back(
-                feedback_message.sequence[i] + feedback_message.sequence[i - 1]);
+            for (int i = 1; i < goal_request->goal.order; ++i) {
+              feedback_message.feedback.sequence.push_back(
+                feedback_message.feedback.sequence[i] +
+                feedback_message.feedback.sequence[i - 1]);
               feedback_publisher->publish(feedback_message);
               client_executor.spin_once();
             }
@@ -137,11 +139,11 @@ protected:
           status_message.status_list[0] = goal_status;
           status_publisher->publish(status_message);
           client_executor.spin_once();
-          response->sequence = feedback_message.sequence;
-          response->action_status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
-          goals.erase(request->action_goal_id.uuid);
+          response->result.sequence = feedback_message.feedback.sequence;
+          response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
+          goals.erase(request->goal_id.uuid);
         } else {
-          response->action_status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
+          response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
         }
       });
     ASSERT_TRUE(result_service != nullptr);
@@ -157,7 +159,7 @@ protected:
         const ActionCancelGoalRequest::SharedPtr request,
         ActionCancelGoalResponse::SharedPtr response)
       {
-        rclcpp_action::GoalID zero_uuid;
+        rclcpp_action::GoalUUID zero_uuid;
         std::fill(zero_uuid.begin(), zero_uuid.end(), 0u);
         const rclcpp::Time cancel_stamp = request->goal_info.stamp;
         bool cancel_all = (
@@ -170,11 +172,11 @@ protected:
           auto goal_response = it->second.second;
           const rclcpp::Time goal_stamp = goal_response->stamp;
           bool cancel_this = (
-            request->goal_info.goal_id.uuid == goal_request->action_goal_id.uuid ||
+            request->goal_info.goal_id.uuid == goal_request->goal_id.uuid ||
             cancel_stamp > goal_stamp);
           if (cancel_all || cancel_this) {
             rclcpp_action::GoalStatus goal_status;
-            goal_status.goal_info.goal_id.uuid = goal_request->action_goal_id.uuid;
+            goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid;
             goal_status.goal_info.stamp = goal_response->stamp;
             goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED;
             status_message.status_list.push_back(goal_status);
@@ -250,7 +252,7 @@ protected:
   const char * const action_name{"fibonacci_test"};
 
   std::map<
-    rclcpp_action::GoalID,
+    rclcpp_action::GoalUUID,
     std::pair<
       typename ActionGoalRequest::SharedPtr,
       typename ActionGoalResponse::SharedPtr>> goals;
@@ -266,19 +268,20 @@ TEST_F(TestClient, construction_and_destruction)
   ASSERT_NO_THROW(rclcpp_action::create_client<ActionType>(client_node, action_name).reset());
 }
 
-TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result)
+TEST_F(TestClient, async_send_goal_no_callbacks)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
 
   ActionGoal bad_goal;
   bad_goal.order = -5;
-  auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true);
+  auto future_goal_handle = action_client->async_send_goal(bad_goal);
   dual_spin_until_future_complete(future_goal_handle);
   EXPECT_EQ(nullptr, future_goal_handle.get().get());
 
   ActionGoal good_goal;
   good_goal.order = 5;
-  future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true);
+  future_goal_handle = action_client->async_send_goal(good_goal);
   dual_spin_until_future_complete(future_goal_handle);
   auto goal_handle = future_goal_handle.get();
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
@@ -287,9 +290,10 @@ TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result)
   EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError);
 }
 
-TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result)
+TEST_F(TestClient, async_send_goal_no_callbacks_wait_for_result)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
 
   ActionGoal goal;
   goal.order = 5;
@@ -298,54 +302,162 @@ TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result)
   auto goal_handle = future_goal_handle.get();
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
   EXPECT_FALSE(goal_handle->is_feedback_aware());
+  EXPECT_FALSE(goal_handle->is_result_aware());
+  auto future_result = action_client->async_get_result(goal_handle);
   EXPECT_TRUE(goal_handle->is_result_aware());
-  auto future_result = goal_handle->async_result();
   dual_spin_until_future_complete(future_result);
-  auto result = future_result.get();
-  ASSERT_EQ(6ul, result.response->sequence.size());
-  EXPECT_EQ(0, result.response->sequence[0]);
-  EXPECT_EQ(1, result.response->sequence[1]);
-  EXPECT_EQ(5, result.response->sequence[5]);
+  auto wrapped_result = future_result.get();
+  ASSERT_EQ(6ul, wrapped_result.result->sequence.size());
+  EXPECT_EQ(0, wrapped_result.result->sequence[0]);
+  EXPECT_EQ(1, wrapped_result.result->sequence[1]);
+  EXPECT_EQ(5, wrapped_result.result->sequence[5]);
 }
 
-TEST_F(TestClient, async_send_goal_with_feedback_and_result)
+TEST_F(TestClient, async_send_goal_with_goal_response_callback_wait_for_result)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  ActionGoal goal;
+  goal.order = 4;
+  bool goal_response_received = false;
+  auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
+  send_goal_ops.goal_response_callback =
+    [&goal_response_received]
+      (std::shared_future<typename ActionGoalHandle::SharedPtr> future) mutable
+    {
+      auto goal_handle = future.get();
+      if (goal_handle) {
+        goal_response_received = true;
+      }
+    };
+  auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_TRUE(goal_response_received);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_FALSE(goal_handle->is_feedback_aware());
+  EXPECT_FALSE(goal_handle->is_result_aware());
+  auto future_result = action_client->async_get_result(goal_handle);
+  EXPECT_TRUE(goal_handle->is_result_aware());
+  dual_spin_until_future_complete(future_result);
+  auto wrapped_result = future_result.get();
+
+  ASSERT_EQ(5u, wrapped_result.result->sequence.size());
+  EXPECT_EQ(3, wrapped_result.result->sequence.back());
+}
+
+TEST_F(TestClient, async_send_goal_with_feedback_callback_wait_for_result)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
 
   ActionGoal goal;
   goal.order = 4;
   int feedback_count = 0;
-  auto future_goal_handle = action_client->async_send_goal(
-    goal,
+  auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
+  send_goal_ops.feedback_callback =
     [&feedback_count](
-      typename ActionGoalHandle::SharedPtr goal_handle,
-      const std::shared_ptr<const ActionFeedback> feedback) mutable
+    typename ActionGoalHandle::SharedPtr goal_handle,
+    const std::shared_ptr<const ActionFeedback> feedback) mutable
     {
       (void)goal_handle;
       (void)feedback;
       feedback_count++;
-    });
+    };
+  auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
   dual_spin_until_future_complete(future_goal_handle);
   auto goal_handle = future_goal_handle.get();
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
   EXPECT_TRUE(goal_handle->is_feedback_aware());
+  EXPECT_FALSE(goal_handle->is_result_aware());
+  auto future_result = action_client->async_get_result(goal_handle);
   EXPECT_TRUE(goal_handle->is_result_aware());
-  auto future_result = goal_handle->async_result();
   dual_spin_until_future_complete(future_result);
-  auto result = future_result.get();
+  auto wrapped_result = future_result.get();
 
-  ASSERT_EQ(5u, result.response->sequence.size());
-  EXPECT_EQ(3, result.response->sequence.back());
+  ASSERT_EQ(5u, wrapped_result.result->sequence.size());
+  EXPECT_EQ(3, wrapped_result.result->sequence.back());
   EXPECT_EQ(5, feedback_count);
 }
 
+TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  ActionGoal goal;
+  goal.order = 4;
+  bool result_callback_received = false;
+  auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
+  send_goal_ops.result_callback =
+    [&result_callback_received](
+    const typename ActionGoalHandle::WrappedResult & result) mutable
+    {
+      if (rclcpp_action::ResultCode::SUCCEEDED == result.code &&
+        result.result->sequence.size() == 5u)
+      {
+        result_callback_received = true;
+      }
+    };
+  auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_FALSE(goal_handle->is_feedback_aware());
+  EXPECT_TRUE(goal_handle->is_result_aware());
+  auto future_result = action_client->async_get_result(goal_handle);
+  dual_spin_until_future_complete(future_result);
+  auto wrapped_result = future_result.get();
+
+  EXPECT_TRUE(result_callback_received);
+  ASSERT_EQ(5u, wrapped_result.result->sequence.size());
+  EXPECT_EQ(3, wrapped_result.result->sequence.back());
+}
+
+TEST_F(TestClient, async_get_result_with_callback)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  ActionGoal goal;
+  goal.order = 4;
+  auto future_goal_handle = action_client->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_NE(goal_handle, nullptr);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_FALSE(goal_handle->is_feedback_aware());
+  EXPECT_FALSE(goal_handle->is_result_aware());
+  bool result_callback_received = false;
+  auto future_result = action_client->async_get_result(
+    goal_handle,
+    [&result_callback_received](
+      const typename ActionGoalHandle::WrappedResult & result) mutable
+    {
+      if (
+        rclcpp_action::ResultCode::SUCCEEDED == result.code &&
+        result.result->sequence.size() == 5u)
+      {
+        result_callback_received = true;
+      }
+    });
+  dual_spin_until_future_complete(future_result);
+  auto wrapped_result = future_result.get();
+
+  EXPECT_TRUE(result_callback_received);
+  ASSERT_EQ(5u, wrapped_result.result->sequence.size());
+  EXPECT_EQ(3, wrapped_result.result->sequence.back());
+}
+
 TEST_F(TestClient, async_cancel_one_goal)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
 
   ActionGoal goal;
   goal.order = 5;
-  auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true);
+  auto future_goal_handle = action_client->async_send_goal(goal);
   dual_spin_until_future_complete(future_goal_handle);
   auto goal_handle = future_goal_handle.get();
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
@@ -356,20 +468,52 @@ TEST_F(TestClient, async_cancel_one_goal)
   EXPECT_TRUE(goal_canceled);
 }
 
+TEST_F(TestClient, async_cancel_one_goal_with_callback)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  ActionGoal goal;
+  goal.order = 5;
+  auto future_goal_handle = action_client->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+
+  bool cancel_response_received = false;
+  auto future_cancel = action_client->async_cancel_goal(
+    goal_handle,
+    [&cancel_response_received, goal_handle](
+      typename ActionGoalHandle::SharedPtr goal_handle_canceled, bool cancel_accepted) mutable
+    {
+      if (
+        goal_handle_canceled->get_goal_id() == goal_handle->get_goal_id() &&
+        cancel_accepted)
+      {
+        cancel_response_received = true;
+      }
+    });
+  dual_spin_until_future_complete(future_cancel);
+  bool goal_canceled = future_cancel.get();
+  EXPECT_TRUE(goal_canceled);
+  EXPECT_TRUE(cancel_response_received);
+}
+
 TEST_F(TestClient, async_cancel_all_goals)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
 
   ActionGoal goal;
   goal.order = 6;
-  auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
+  auto future_goal_handle0 = action_client->async_send_goal(goal);
   dual_spin_until_future_complete(future_goal_handle0);
   auto goal_handle0 = future_goal_handle0.get();
 
   ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
 
   goal.order = 8;
-  auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
+  auto future_goal_handle1 = action_client->async_send_goal(goal);
   dual_spin_until_future_complete(future_goal_handle1);
   auto goal_handle1 = future_goal_handle1.get();
 
@@ -390,20 +534,70 @@ TEST_F(TestClient, async_cancel_all_goals)
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
 }
 
-TEST_F(TestClient, async_cancel_some_goals)
+TEST_F(TestClient, async_cancel_all_goals_with_callback)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
 
   ActionGoal goal;
   goal.order = 6;
-  auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
+  auto future_goal_handle0 = action_client->async_send_goal(goal);
   dual_spin_until_future_complete(future_goal_handle0);
   auto goal_handle0 = future_goal_handle0.get();
 
   ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
 
   goal.order = 8;
-  auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
+  auto future_goal_handle1 = action_client->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle1);
+  auto goal_handle1 = future_goal_handle1.get();
+
+  if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) {
+    goal_handle0.swap(goal_handle1);
+  }
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
+
+  bool cancel_callback_received = false;
+  auto future_cancel_all = action_client->async_cancel_all_goals(
+    [&cancel_callback_received, goal_handle0, goal_handle1](
+      ActionCancelGoalResponse::SharedPtr response)
+    {
+      if (
+        response &&
+        2ul == response->goals_canceling.size() &&
+        goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid &&
+        goal_handle1->get_goal_id() == response->goals_canceling[1].goal_id.uuid)
+      {
+        cancel_callback_received = true;
+      }
+    });
+  dual_spin_until_future_complete(future_cancel_all);
+  auto cancel_response = future_cancel_all.get();
+
+  EXPECT_TRUE(cancel_callback_received);
+  ASSERT_EQ(2ul, cancel_response->goals_canceling.size());
+  EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
+  EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
+}
+
+TEST_F(TestClient, async_cancel_some_goals)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  ActionGoal goal;
+  goal.order = 6;
+  auto future_goal_handle0 = action_client->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle0);
+  auto goal_handle0 = future_goal_handle0.get();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
+
+  goal.order = 8;
+  auto future_goal_handle1 = action_client->async_send_goal(goal);
   dual_spin_until_future_complete(future_goal_handle1);
   auto goal_handle1 = future_goal_handle1.get();
 
@@ -418,3 +612,45 @@ TEST_F(TestClient, async_cancel_some_goals)
   EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
   EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
 }
+
+TEST_F(TestClient, async_cancel_some_goals_with_callback)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  ActionGoal goal;
+  goal.order = 6;
+  auto future_goal_handle0 = action_client->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle0);
+  auto goal_handle0 = future_goal_handle0.get();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
+
+  goal.order = 8;
+  auto future_goal_handle1 = action_client->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle1);
+  auto goal_handle1 = future_goal_handle1.get();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
+
+  bool cancel_callback_received = false;
+  auto future_cancel_some = action_client->async_cancel_goals_before(
+    goal_handle1->get_goal_stamp(),
+    [&cancel_callback_received, goal_handle0](ActionCancelGoalResponse::SharedPtr response)
+    {
+      if (
+        response &&
+        1ul == response->goals_canceling.size() &&
+        goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid)
+      {
+        cancel_callback_received = true;
+      }
+    });
+  dual_spin_until_future_complete(future_cancel_some);
+  auto cancel_response = future_cancel_some.get();
+
+  EXPECT_TRUE(cancel_callback_received);
+  ASSERT_EQ(1ul, cancel_response->goals_canceling.size());
+  EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
+}

rclcpp_action/test/test_server.cpp

@@ -26,7 +26,7 @@
 #include "rclcpp_action/server.hpp"
 
 using Fibonacci = test_msgs::action::Fibonacci;
-using GoalID = rclcpp_action::GoalID;
+using GoalUUID = rclcpp_action::GoalUUID;
 
 class TestServer : public ::testing::Test
 {
@@ -36,16 +36,16 @@ protected:
     rclcpp::init(0, nullptr);
   }
 
-  std::shared_ptr<Fibonacci::GoalRequestService::Request>
-  send_goal_request(rclcpp::Node::SharedPtr node, GoalID uuid)
+  std::shared_ptr<Fibonacci::Impl::SendGoalService::Request>
+  send_goal_request(rclcpp::Node::SharedPtr node, GoalUUID uuid)
   {
-    auto client = node->create_client<Fibonacci::GoalRequestService>(
+    auto client = node->create_client<Fibonacci::Impl::SendGoalService>(
       "fibonacci/_action/send_goal");
     if (!client->wait_for_service(std::chrono::seconds(20))) {
       throw std::runtime_error("send goal service didn't become available");
     }
-    auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();
-    request->action_goal_id.uuid = uuid;
+    auto request = std::make_shared<Fibonacci::Impl::SendGoalService::Request>();
+    request->goal_id.uuid = uuid;
     auto future = client->async_send_request(request);
     if (rclcpp::executor::FutureReturnCode::SUCCESS !=
       rclcpp::spin_until_future_complete(node, future))
@@ -56,14 +56,14 @@ protected:
   }
 
   void
-  send_cancel_request(rclcpp::Node::SharedPtr node, GoalID uuid)
+  send_cancel_request(rclcpp::Node::SharedPtr node, GoalUUID uuid)
   {
-    auto cancel_client = node->create_client<Fibonacci::CancelGoalService>(
+    auto cancel_client = node->create_client<Fibonacci::Impl::CancelGoalService>(
       "fibonacci/_action/cancel_goal");
     if (!cancel_client->wait_for_service(std::chrono::seconds(20))) {
       throw std::runtime_error("cancel goal service didn't become available");
     }
-    auto request = std::make_shared<Fibonacci::CancelGoalService::Request>();
+    auto request = std::make_shared<Fibonacci::Impl::CancelGoalService::Request>();
     request->goal_info.goal_id.uuid = uuid;
     auto future = cancel_client->async_send_request(request);
     if (rclcpp::executor::FutureReturnCode::SUCCESS !=
@@ -80,7 +80,7 @@ TEST_F(TestServer, construction_and_destruction)
 
   using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
   auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
-      [](const GoalID &, std::shared_ptr<const Fibonacci::Goal>) {
+      [](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
         return rclcpp_action::GoalResponse::REJECT;
       },
       [](std::shared_ptr<GoalHandle>) {
@@ -93,10 +93,10 @@ TEST_F(TestServer, construction_and_destruction)
 TEST_F(TestServer, handle_goal_called)
 {
   auto node = std::make_shared<rclcpp::Node>("handle_goal_node", "/rclcpp_action/handle_goal");
-  GoalID received_uuid;
+  GoalUUID received_uuid;
 
   auto handle_goal = [&received_uuid](
-    const GoalID & uuid, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID & uuid, std::shared_ptr<const Fibonacci::Goal>)
     {
       received_uuid = uuid;
       return rclcpp_action::GoalResponse::REJECT;
@@ -114,15 +114,15 @@ TEST_F(TestServer, handle_goal_called)
   // Create a client that calls the goal request service
   // Make sure the UUID received is the same as the one sent
 
-  auto client = node->create_client<Fibonacci::GoalRequestService>(
+  auto client = node->create_client<Fibonacci::Impl::SendGoalService>(
     "fibonacci/_action/send_goal");
 
   ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20)));
 
-  auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();
+  auto request = std::make_shared<Fibonacci::Impl::SendGoalService::Request>();
 
-  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
-  request->action_goal_id.uuid = uuid;
+  const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+  request->goal_id.uuid = uuid;
 
   auto future = client->async_send_request(request);
   ASSERT_EQ(
@@ -135,10 +135,10 @@ TEST_F(TestServer, handle_goal_called)
 TEST_F(TestServer, handle_accepted_called)
 {
   auto node = std::make_shared<rclcpp::Node>("handle_exec_node", "/rclcpp_action/handle_accepted");
-  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+  const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
 
   auto handle_goal = [](
-    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
     {
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
@@ -164,16 +164,16 @@ TEST_F(TestServer, handle_accepted_called)
   ASSERT_TRUE(received_handle);
   ASSERT_TRUE(received_handle->is_active());
   EXPECT_EQ(uuid, received_handle->get_goal_id());
-  EXPECT_EQ(*request, *(received_handle->get_goal()));
+  EXPECT_EQ(request->goal, *(received_handle->get_goal()));
 }
 
 TEST_F(TestServer, handle_cancel_called)
 {
   auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel");
-  const GoalID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+  const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
 
   auto handle_goal = [](
-    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
     {
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
@@ -210,10 +210,10 @@ TEST_F(TestServer, handle_cancel_called)
 TEST_F(TestServer, publish_status_accepted)
 {
   auto node = std::make_shared<rclcpp::Node>("status_accept_node", "/rclcpp_action/status_accept");
-  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}};
+  const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}};
 
   auto handle_goal = [](
-    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
     {
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
@@ -271,10 +271,10 @@ TEST_F(TestServer, publish_status_accepted)
 TEST_F(TestServer, publish_status_canceling)
 {
   auto node = std::make_shared<rclcpp::Node>("status_cancel_node", "/rclcpp_action/status_cancel");
-  const GoalID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}};
+  const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}};
 
   auto handle_goal = [](
-    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
     {
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
@@ -326,10 +326,10 @@ TEST_F(TestServer, publish_status_canceling)
 TEST_F(TestServer, publish_status_canceled)
 {
   auto node = std::make_shared<rclcpp::Node>("status_canceled", "/rclcpp_action/status_canceled");
-  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+  const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
 
   auto handle_goal = [](
-    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
     {
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
@@ -364,7 +364,7 @@ TEST_F(TestServer, publish_status_canceled)
   send_goal_request(node, uuid);
   send_cancel_request(node, uuid);
 
-  received_handle->set_canceled(std::make_shared<Fibonacci::Result>());
+  received_handle->canceled(std::make_shared<Fibonacci::Result>());
 
   // 10 seconds
   const size_t max_tries = 10 * 1000 / 100;
@@ -383,10 +383,10 @@ TEST_F(TestServer, publish_status_canceled)
 TEST_F(TestServer, publish_status_succeeded)
 {
   auto node = std::make_shared<rclcpp::Node>("status_succeeded", "/rclcpp_action/status_succeeded");
-  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+  const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
 
   auto handle_goal = [](
-    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
     {
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
@@ -419,7 +419,7 @@ TEST_F(TestServer, publish_status_succeeded)
     });
 
   send_goal_request(node, uuid);
-  received_handle->set_succeeded(std::make_shared<Fibonacci::Result>());
+  received_handle->succeed(std::make_shared<Fibonacci::Result>());
 
   // 10 seconds
   const size_t max_tries = 10 * 1000 / 100;
@@ -438,10 +438,10 @@ TEST_F(TestServer, publish_status_succeeded)
 TEST_F(TestServer, publish_status_aborted)
 {
   auto node = std::make_shared<rclcpp::Node>("status_aborted", "/rclcpp_action/status_aborted");
-  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+  const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
 
   auto handle_goal = [](
-    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
     {
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
@@ -474,7 +474,7 @@ TEST_F(TestServer, publish_status_aborted)
     });
 
   send_goal_request(node, uuid);
-  received_handle->set_aborted(std::make_shared<Fibonacci::Result>());
+  received_handle->abort(std::make_shared<Fibonacci::Result>());
 
   // 10 seconds
   const size_t max_tries = 10 * 1000 / 100;
@@ -493,10 +493,10 @@ TEST_F(TestServer, publish_status_aborted)
 TEST_F(TestServer, publish_feedback)
 {
   auto node = std::make_shared<rclcpp::Node>("pub_feedback", "/rclcpp_action/pub_feedback");
-  const GoalID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}};
+  const GoalUUID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}};
 
   auto handle_goal = [](
-    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
     {
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
@@ -521,7 +521,7 @@ TEST_F(TestServer, publish_feedback)
   (void)as;
 
   // Subscribe to feedback messages
-  using FeedbackT = Fibonacci::Feedback;
+  using FeedbackT = Fibonacci::Impl::FeedbackMessage;
   std::vector<FeedbackT::SharedPtr> received_msgs;
   auto subscriber = node->create_subscription<FeedbackT>(
     "fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg)
@@ -531,7 +531,7 @@ TEST_F(TestServer, publish_feedback)
 
   send_goal_request(node, uuid);
 
-  auto sent_message = std::make_shared<FeedbackT>();
+  auto sent_message = std::make_shared<Fibonacci::Feedback>();
   sent_message->sequence = {1, 1, 2, 3, 5};
   received_handle->publish_feedback(sent_message);
 
@@ -544,17 +544,16 @@ TEST_F(TestServer, publish_feedback)
 
   ASSERT_EQ(1u, received_msgs.size());
   auto & msg = received_msgs.back();
-  ASSERT_EQ(sent_message->sequence, msg->sequence);
-  ASSERT_EQ(uuid, msg->action_goal_id.uuid);
+  ASSERT_EQ(sent_message->sequence, msg->feedback.sequence);
 }
 
 TEST_F(TestServer, get_result)
 {
   auto node = std::make_shared<rclcpp::Node>("get_result", "/rclcpp_action/get_result");
-  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}};
+  const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}};
 
   auto handle_goal = [](
-    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
     {
       return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     };
@@ -581,36 +580,36 @@ TEST_F(TestServer, get_result)
   send_goal_request(node, uuid);
 
   // Send result request
-  auto result_client = node->create_client<Fibonacci::GoalResultService>(
+  auto result_client = node->create_client<Fibonacci::Impl::GetResultService>(
     "fibonacci/_action/get_result");
   if (!result_client->wait_for_service(std::chrono::seconds(20))) {
     throw std::runtime_error("get result service didn't become available");
   }
-  auto request = std::make_shared<Fibonacci::GoalResultService::Request>();
-  request->action_goal_id.uuid = uuid;
+  auto request = std::make_shared<Fibonacci::Impl::GetResultService::Request>();
+  request->goal_id.uuid = uuid;
   auto future = result_client->async_send_request(request);
 
   // Send a result
   auto result = std::make_shared<Fibonacci::Result>();
   result->sequence = {5, 8, 13, 21};
-  received_handle->set_succeeded(result);
+  received_handle->succeed(result);
 
   // Wait for the result request to be received
   ASSERT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS,
     rclcpp::spin_until_future_complete(node, future));
 
   auto response = future.get();
-  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->action_status);
-  EXPECT_EQ(result->sequence, response->sequence);
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status);
+  EXPECT_EQ(result->sequence, response->result.sequence);
 }
 
 TEST_F(TestServer, deferred_execution)
 {
   auto node = std::make_shared<rclcpp::Node>("defer_exec", "/rclcpp_action/defer_exec");
-  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+  const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
 
   auto handle_goal = [](
-    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>)
     {
       return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
     };
@@ -638,6 +637,6 @@ TEST_F(TestServer, deferred_execution)
 
   EXPECT_TRUE(received_handle->is_active());
   EXPECT_FALSE(received_handle->is_executing());
-  received_handle->set_executing();
+  received_handle->execute();
   EXPECT_TRUE(received_handle->is_executing());
 }

rclcpp_components/CHANGELOG.rst

@@ -0,0 +1,29 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rclcpp_components
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.7.1 (2019-04-26)
+------------------
+
+0.7.0 (2019-04-14)
+------------------
+* Introduce rclcpp_components to implement composition (`#665 <https://github.com/ros2/rclcpp/issues/665>`_)
+* Contributors: Michael Carroll
+
+0.6.2 (2018-12-12)
+------------------
+
+0.6.1 (2018-12-06)
+------------------
+
+0.6.0 (2018-11-19)
+------------------
+
+0.5.1 (2018-06-28)
+------------------
+
+0.5.0 (2018-06-25)
+------------------
+
+0.4.0 (2017-12-08)
+------------------

rclcpp_components/CMakeLists.txt

@@ -0,0 +1,122 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(rclcpp_components)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(class_loader REQUIRED)
+find_package(composition_interfaces REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rcpputils REQUIRED)
+
+include_directories(include)
+
+add_library(
+  component_manager
+  STATIC
+  src/component_manager.cpp
+)
+ament_target_dependencies(component_manager
+  "ament_index_cpp"
+  "class_loader"
+  "composition_interfaces"
+  "rclcpp"
+  "rcpputils"
+)
+
+add_executable(
+  component_container
+  src/component_container.cpp
+)
+target_link_libraries(component_container component_manager)
+ament_target_dependencies(component_container
+  "rclcpp"
+)
+
+add_executable(
+  component_container_mt
+  src/component_container_mt.cpp
+)
+target_link_libraries(component_container_mt component_manager)
+ament_target_dependencies(component_container_mt
+  "rclcpp"
+)
+
+if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  target_link_libraries(component_container "stdc++fs")
+  target_link_libraries(component_container_mt "stdc++fs")
+endif()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  set(components "")
+  add_library(test_component SHARED test/components/test_component.cpp)
+  ament_target_dependencies(test_component
+    "class_loader"
+    "rclcpp"
+    "rclcpp_components")
+  #rclcpp_components_register_nodes(test_component "test_rclcpp_components::TestComponent")
+  set(components "${components}test_rclcpp_components::TestComponentFoo;$<TARGET_FILE:test_component>\n")
+  set(components "${components}test_rclcpp_components::TestComponentBar;$<TARGET_FILE:test_component>\n")
+  set(components "${components}test_rclcpp_components::TestComponentNoNode;$<TARGET_FILE:test_component>\n")
+
+  file(GENERATE
+    OUTPUT
+    "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$<CONFIG>/share/ament_index/resource_index/rclcpp_components/${PROJECT_NAME}"
+    CONTENT "${components}")
+
+  set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
+  if(WIN32)
+    set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
+  endif()
+
+  ament_add_gtest(test_component_manager test/test_component_manager.cpp
+    APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$<CONFIG>
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_component_manager)
+    target_link_libraries(test_component_manager component_manager)
+    target_include_directories(test_component_manager PRIVATE src)
+  endif()
+
+  ament_add_gtest(test_component_manager_api test/test_component_manager_api.cpp
+    APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$<CONFIG>
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_component_manager_api)
+    target_link_libraries(test_component_manager_api component_manager)
+    target_include_directories(test_component_manager_api PRIVATE src)
+  endif()
+endif()
+
+# Install executables
+install(
+  TARGETS component_container component_container_mt
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install include directories
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+# Install cmake
+install(
+  DIRECTORY cmake
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# specific order: dependents before dependencies
+ament_export_include_directories(include)
+ament_export_dependencies(class_loader)
+ament_export_dependencies(rclcpp)
+ament_package(CONFIG_EXTRAS rclcpp_components-extras.cmake)

rclcpp_components/cmake/rclcpp_components_package_hook.cmake

@@ -1,4 +1,4 @@
-# Copyright 2016 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.
@@ -14,4 +14,5 @@
 
 # register node plugins
 ament_index_register_resource(
-  "node_plugin" CONTENT "${_RCLCPP__NODE_PLUGINS}")
+  "rclcpp_components" CONTENT "${_RCLCPP_COMPONENTS__NODES}")
+

rclcpp_components/cmake/rclcpp_components_register_nodes.cmake

@@ -1,4 +1,4 @@
-# Copyright 2016 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.
@@ -13,38 +13,38 @@
 # limitations under the License.
 
 #
-# Register a node plugin with the ament resource index.
+# Register an rclcpp component with the ament resource index.
 #
-# The passed library can contain multiple plugins extending the node interface.
+# The passed library can contain multiple nodes each registered via macro.
 #
 # :param target: the shared library target
 # :type target: string
 # :param ARGN: the unique plugin names being exported using class_loader
 # :type ARGN: list of strings
 #
-macro(rclcpp_register_node_plugins target)
+macro(rclcpp_components_register_nodes target)
   if(NOT TARGET ${target})
     message(
       FATAL_ERROR
-      "rclcpp_register_node_plugins() first argument "
+      "rclcpp_components_register_nodes() first argument "
       "'${target}' is not a target")
   endif()
   get_target_property(_target_type ${target} TYPE)
   if(NOT _target_type STREQUAL "SHARED_LIBRARY")
     message(
       FATAL_ERROR
-      "rclcpp_register_node_plugins() first argument "
+      "rclcpp_components_register_nodes() first argument "
       "'${target}' is not a shared library target")
   endif()
 
   if(${ARGC} GREATER 0)
-    _rclcpp_register_package_hook()
+    _rclcpp_components_register_package_hook()
     set(_unique_names)
     foreach(_arg ${ARGN})
       if(_arg IN_LIST _unique_names)
         message(
           FATAL_ERROR
-          "rclcpp_register_node_plugins() the plugin names "
+          "rclcpp_components_register_nodes() the plugin names "
           "must be unique (multiple '${_arg}')")
       endif()
       list(APPEND _unique_names "${_arg}")
@@ -54,8 +54,9 @@ macro(rclcpp_register_node_plugins target)
       else()
         set(_path "lib")
       endif()
-      set(_RCLCPP__NODE_PLUGINS
-        "${_RCLCPP__NODE_PLUGINS}${_arg};${_path}/$<TARGET_FILE_NAME:${target}>\n")
+      set(_RCLCPP_COMPONENTS__NODES
+        "${_RCLCPP_COMPONENTS__NODES}${_arg};${_path}/$<TARGET_FILE_NAME:${target}>\n")
     endforeach()
   endif()
 endmacro()
+

rclcpp_components/include/rclcpp_components/node_factory.hpp

@@ -0,0 +1,46 @@
+// 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_COMPONENTS__NODE_FACTORY_HPP__
+#define RCLCPP_COMPONENTS__NODE_FACTORY_HPP__
+
+#include "rclcpp_components/node_instance_wrapper.hpp"
+
+namespace rclcpp_components
+{
+
+/// The NodeFactory interface is used by the class loader to instantiate components.
+/**
+ * The NodeFactory interface serves two purposes:
+ *  * It allows for classes not derived from `rclcpp::Node` to be used as components.
+ *  * It allows derived constructors to be called when components are loaded.
+ */
+class NodeFactory
+{
+public:
+  NodeFactory() = default;
+
+  virtual ~NodeFactory() = default;
+
+  /// Create an instance of a component
+  /**
+   * \param[in] options Additional options used in the construction of the component.
+   */
+  virtual
+  NodeInstanceWrapper
+  create_node_instance(const rclcpp::NodeOptions & options) = 0;
+};
+}  // namespace rclcpp_components
+
+#endif  // RCLCPP_COMPONENTS__NODE_FACTORY_HPP__

rclcpp_components/include/rclcpp_components/node_factory_template.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_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__
+#define RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__
+
+#include <functional>
+#include <memory>
+
+#include "rclcpp_components/node_factory.hpp"
+
+namespace rclcpp_components
+{
+
+/// NodeFactoryTemplate is a convenience class for instantiating components.
+/**
+ * The NodeFactoryTemplate class can be used to provide the NodeFactory interface for
+ * components that implement a single-argument constructor and `get_node_base_interface`.
+ */
+template<typename NodeT>
+class NodeFactoryTemplate : public NodeFactory
+{
+public:
+  NodeFactoryTemplate() = default;
+  virtual ~NodeFactoryTemplate() = default;
+
+  /// Create an instance of a component
+  /**
+   * \param[in] options Additional options used in the construction of the component.
+   */
+  NodeInstanceWrapper
+  create_node_instance(const rclcpp::NodeOptions & options) override
+  {
+    auto node = std::make_shared<NodeT>(options);
+
+    return NodeInstanceWrapper(
+      node, std::bind(&NodeT::get_node_base_interface, node));
+  }
+};
+}  // namespace rclcpp_components
+
+#endif  // RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__

rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp

@@ -0,0 +1,71 @@
+// 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_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__
+#define RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__
+
+#include <functional>
+#include <memory>
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+
+namespace rclcpp_components
+{
+/// The NodeInstanceWrapper encapsulates the node instance.
+class NodeInstanceWrapper
+{
+public:
+  using NodeBaseInterfaceGetter = std::function<
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr(const std::shared_ptr<void> &)>;
+
+  NodeInstanceWrapper()
+  : node_instance_(nullptr)
+  {}
+
+  NodeInstanceWrapper(
+    std::shared_ptr<void> node_instance,
+    NodeBaseInterfaceGetter node_base_interface_getter)
+  : node_instance_(node_instance), node_base_interface_getter_(node_base_interface_getter)
+  {}
+
+  /// Get a type-erased pointer to the original Node instance
+  /**
+   * This is only for debugging and special cases.
+   * For most cases `get_node_base_interface` will be sufficient.
+   *
+   * \return Shared pointer to the encapsulated Node instance.
+   */
+  const std::shared_ptr<void>
+  get_node_instance() const
+  {
+    return node_instance_;
+  }
+
+  /// Get NodeBaseInterface pointer for the encapsulated Node Instance.
+  /**
+   * \return Shared NodeBaseInterface pointer of the encapsulated Node instance.
+   */
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  get_node_base_interface()
+  {
+    return node_base_interface_getter_(node_instance_);
+  }
+
+private:
+  std::shared_ptr<void> node_instance_;
+  NodeBaseInterfaceGetter node_base_interface_getter_;
+};
+}  // namespace rclcpp_components
+
+#endif  // RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__

rclcpp_components/include/rclcpp_components/register_node_macro.hpp

@@ -0,0 +1,37 @@
+// 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_COMPONENTS__REGISTER_NODE_MACRO_HPP__
+#define RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__
+
+#include "class_loader/class_loader.hpp"
+#include "rclcpp_components/node_factory_template.hpp"
+
+/// Register a component that can be dynamically loaded at runtime.
+/**
+ * The registration macro should appear once per component per library.
+ * The macro should appear in a single translation unit.
+ *
+ * Valid arguments for NodeClass shall:
+ *  * Have a constructor that takes a single argument that is a `rclcpp::NodeOptions` instance.
+ *  * Have a method of of the signature:
+ *      `rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface`
+ *
+ * Note: NodeClass does not need to inherit from `rclcpp::Node`, but it is the easiest way.
+ */
+#define RCLCPP_COMPONENTS_REGISTER_NODE(NodeClass) \
+  CLASS_LOADER_REGISTER_CLASS(rclcpp_components::NodeFactoryTemplate<NodeClass>, \
+    rclcpp_components::NodeFactory)
+
+#endif  // RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__

rclcpp_components/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>rclcpp_components</name>
+  <version>0.7.1</version>
+  <description>Package containing tools for dynamically loadable components</description>
+  <maintainer email="michael@openrobotics.org">Michael Carroll</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <build_depend>ament_index_cpp</build_depend>
+  <build_depend>class_loader</build_depend>
+  <build_depend>composition_interfaces</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>rcpputils</build_depend>
+
+  <exec_depend>ament_index_cpp</exec_depend>
+  <exec_depend>class_loader</exec_depend>
+  <exec_depend>composition_interfaces</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>std_msgs</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
+

rclcpp_components/rclcpp_components-extras.cmake

@@ -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,18 +12,18 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-# copied from rclcpp/rclcpp-extras.cmake
+# copied from rclcpp_components/rclcpp_components-extras.cmake
 
-# register ament_package() hook for node plugins once
-macro(_rclcpp_register_package_hook)
-  if(NOT DEFINED _RCLCPP_PACKAGE_HOOK_REGISTERED)
-    set(_RCLCPP_PACKAGE_HOOK_REGISTERED TRUE)
+# register ament_package() hook for node plugins once.
+macro(_rclcpp_components_register_package_hook)
+  if(NOT DEFINED _RCLCPP_COMPONENTS_PACKAGE_HOOK_REGISTERED)
+    set(_RCLCPP_COMPONENTS_PACKAGE_HOOK_REGISTERED TRUE)
 
     find_package(ament_cmake_core QUIET REQUIRED)
-    ament_register_extension("ament_package" "rclcpp"
-      "rclcpp_package_hook.cmake")
+    ament_register_extension("ament_package" "rclcpp_components"
+      "rclcpp_components_package_hook.cmake")
   endif()
 endmacro()
 
-include("${rclcpp_DIR}/rclcpp_create_node_main.cmake")
-include("${rclcpp_DIR}/rclcpp_register_node_plugins.cmake")
+include("${rclcpp_components_DIR}/rclcpp_components_register_nodes.cmake")
+

rclcpp_components/src/component_container.cpp

@@ -0,0 +1,29 @@
+// 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 <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "component_manager.hpp"
+
+int main(int argc, char * argv[])
+{
+  /// Component container with a single-threaded executor.
+  rclcpp::init(argc, argv);
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto node = std::make_shared<rclcpp_components::ComponentManager>(exec);
+  exec->add_node(node);
+  exec->spin();
+}

rclcpp_components/src/component_container_mt.cpp

@@ -0,0 +1,29 @@
+// 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 <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "component_manager.hpp"
+
+int main(int argc, char * argv[])
+{
+  /// Component container with a multi-threaded executor.
+  rclcpp::init(argc, argv);
+  auto exec = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+  auto node = std::make_shared<rclcpp_components::ComponentManager>(exec);
+  exec->add_node(node);
+  exec->spin();
+}

rclcpp_components/src/component_manager.cpp

@@ -0,0 +1,239 @@
+// 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 "component_manager.hpp"
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "ament_index_cpp/get_resource.hpp"
+#include "rcpputils/filesystem_helper.hpp"
+#include "rcpputils/split.hpp"
+
+using namespace std::placeholders;
+
+namespace rclcpp_components
+{
+
+ComponentManager::ComponentManager(
+  std::weak_ptr<rclcpp::executor::Executor> executor)
+: Node("ComponentManager"),
+  executor_(executor)
+{
+  loadNode_srv_ = create_service<LoadNode>("~/_container/load_node",
+      std::bind(&ComponentManager::OnLoadNode, this, _1, _2, _3));
+  unloadNode_srv_ = create_service<UnloadNode>("~/_container/unload_node",
+      std::bind(&ComponentManager::OnUnloadNode, this, _1, _2, _3));
+  listNodes_srv_ = create_service<ListNodes>("~/_container/list_nodes",
+      std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3));
+}
+
+ComponentManager::~ComponentManager()
+{
+  if (node_wrappers_.size()) {
+    RCLCPP_DEBUG(get_logger(), "Removing components from executor");
+    if (auto exec = executor_.lock()) {
+      for (auto & wrapper : node_wrappers_) {
+        exec->remove_node(wrapper.second.get_node_base_interface());
+      }
+    }
+  }
+}
+
+std::vector<ComponentManager::ComponentResource>
+ComponentManager::get_component_resources(const std::string & package_name) const
+{
+  std::string content;
+  std::string base_path;
+  if (!ament_index_cpp::get_resource(
+      "rclcpp_components", package_name, content, &base_path))
+  {
+    throw ComponentManagerException("Could not find requested resource in ament index");
+  }
+
+  std::vector<ComponentResource> resources;
+  std::vector<std::string> lines = rcpputils::split(content, '\n', true);
+  for (const auto & line : lines) {
+    std::vector<std::string> parts = rcpputils::split(line, ';');
+    if (parts.size() != 2) {
+      throw ComponentManagerException("Invalid resource entry");
+    }
+
+    std::string library_path = parts[1];
+    if (!rcpputils::fs::path(library_path).is_absolute()) {
+      library_path = base_path + "/" + library_path;
+    }
+    resources.push_back({parts[0], library_path});
+  }
+  return resources;
+}
+
+std::shared_ptr<rclcpp_components::NodeFactory>
+ComponentManager::create_component_factory(const ComponentResource & resource)
+{
+  std::string library_path = resource.second;
+  std::string class_name = resource.first;
+  std::string fq_class_name = "rclcpp_components::NodeFactoryTemplate<" + class_name + ">";
+
+  class_loader::ClassLoader * loader;
+  if (loaders_.find(library_path) == loaders_.end()) {
+    RCLCPP_INFO(get_logger(), "Load Library: %s", library_path.c_str());
+    try {
+      loaders_[library_path] = std::make_unique<class_loader::ClassLoader>(library_path);
+    } catch (const std::exception & ex) {
+      throw ComponentManagerException("Failed to load library: " + std::string(ex.what()));
+    } catch (...) {
+      throw ComponentManagerException("Failed to load library");
+    }
+  }
+  loader = loaders_[library_path].get();
+
+  auto classes = loader->getAvailableClasses<rclcpp_components::NodeFactory>();
+  for (const auto & clazz : classes) {
+    RCLCPP_INFO(get_logger(), "Found class: %s", clazz.c_str());
+    if (clazz == class_name || clazz == fq_class_name) {
+      RCLCPP_INFO(get_logger(), "Instantiate class: %s", clazz.c_str());
+      return loader->createInstance<rclcpp_components::NodeFactory>(clazz);
+    }
+  }
+  return {};
+}
+
+void
+ComponentManager::OnLoadNode(
+  const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<LoadNode::Request> request,
+  std::shared_ptr<LoadNode::Response> response)
+{
+  (void) request_header;
+
+  try {
+    auto resources = get_component_resources(request->package_name);
+
+    for (const auto & resource : resources) {
+      if (resource.first != request->plugin_name) {
+        continue;
+      }
+      auto factory = create_component_factory(resource);
+
+      if (factory == nullptr) {
+        continue;
+      }
+
+      std::vector<rclcpp::Parameter> parameters;
+      for (const auto & p : request->parameters) {
+        parameters.push_back(rclcpp::Parameter::from_parameter_msg(p));
+      }
+
+      std::vector<std::string> remap_rules {request->remap_rules};
+
+      if (!request->node_name.empty()) {
+        remap_rules.push_back("__node:=" + request->node_name);
+      }
+
+      if (!request->node_namespace.empty()) {
+        remap_rules.push_back("__ns:=" + request->node_namespace);
+      }
+
+      auto options = rclcpp::NodeOptions()
+        .initial_parameters(parameters)
+        .arguments(remap_rules);
+
+      auto node_id = unique_id++;
+
+      if (0 == node_id) {
+        // This puts a technical limit on the number of times you can add a component.
+        // But even if you could add (and remove) them at 1 kHz (very optimistic rate)
+        // it would still be a very long time before you could exhaust the pool of id's:
+        //   2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years
+        // So around 585 million years. Even at 1 GHz, it would take 585 years.
+        // I think it's safe to avoid trying to handle overflow.
+        // If we roll over then it's most likely a bug.
+        throw std::overflow_error("exhausted the unique ids for components in this process");
+      }
+
+      try {
+        node_wrappers_[node_id] = factory->create_node_instance(options);
+      } catch (...) {
+        // In the case that the component constructor throws an exception,
+        // rethrow into the following catch block.
+        throw ComponentManagerException("Component constructor threw an exception");
+      }
+
+      auto node = node_wrappers_[node_id].get_node_base_interface();
+      if (auto exec = executor_.lock()) {
+        exec->add_node(node, true);
+      }
+      response->full_node_name = node->get_fully_qualified_name();
+      response->unique_id = node_id;
+      response->success = true;
+      return;
+    }
+    RCLCPP_ERROR(
+      get_logger(), "Failed to find class with the requested plugin name '%s' in "
+      "the loaded library",
+      request->plugin_name.c_str());
+    response->error_message = "Failed to find class with the requested plugin name.";
+    response->success = false;
+  } catch (const ComponentManagerException & ex) {
+    RCLCPP_ERROR(get_logger(), ex.what());
+    response->error_message = ex.what();
+    response->success = false;
+  }
+}
+
+void
+ComponentManager::OnUnloadNode(
+  const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<UnloadNode::Request> request,
+  std::shared_ptr<UnloadNode::Response> response)
+{
+  (void) request_header;
+
+  auto wrapper = node_wrappers_.find(request->unique_id);
+
+  if (wrapper == node_wrappers_.end()) {
+    response->success = false;
+    std::stringstream ss;
+    ss << "No node found with unique_id: " << request->unique_id;
+    response->error_message = ss.str();
+    RCLCPP_WARN(get_logger(), ss.str());
+  } else {
+    if (auto exec = executor_.lock()) {
+      exec->remove_node(wrapper->second.get_node_base_interface());
+    }
+    node_wrappers_.erase(wrapper);
+    response->success = true;
+  }
+}
+
+void
+ComponentManager::OnListNodes(
+  const std::shared_ptr<rmw_request_id_t> request_header,
+  const std::shared_ptr<ListNodes::Request> request,
+  std::shared_ptr<ListNodes::Response> response)
+{
+  (void) request_header;
+  (void) request;
+
+  for (auto & wrapper : node_wrappers_) {
+    response->unique_ids.push_back(wrapper.first);
+    response->full_node_names.push_back(
+      wrapper.second.get_node_base_interface()->get_fully_qualified_name());
+  }
+}
+
+}  // namespace rclcpp_components

rclcpp_components/src/component_manager.hpp

@@ -0,0 +1,104 @@
+// 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 COMPONENT_MANAGER_HPP__
+#define COMPONENT_MANAGER_HPP__
+
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "class_loader/class_loader.hpp"
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/node_options.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "composition_interfaces/srv/load_node.hpp"
+#include "composition_interfaces/srv/unload_node.hpp"
+#include "composition_interfaces/srv/list_nodes.hpp"
+
+#include "rclcpp_components/node_factory.hpp"
+
+namespace rclcpp_components
+{
+
+class ComponentManagerException : public std::runtime_error
+{
+public:
+  explicit ComponentManagerException(const std::string & error_desc)
+  : std::runtime_error(error_desc) {}
+};
+
+class ComponentManager : public rclcpp::Node
+{
+public:
+  using LoadNode = composition_interfaces::srv::LoadNode;
+  using UnloadNode = composition_interfaces::srv::UnloadNode;
+  using ListNodes = composition_interfaces::srv::ListNodes;
+
+  /// Represents a component resource.
+  /**
+   * Is a pair of class name (for class loader) and library path (absolute)
+   */
+  using ComponentResource = std::pair<std::string, std::string>;
+
+  ComponentManager(
+    std::weak_ptr<rclcpp::executor::Executor> executor);
+
+  ~ComponentManager();
+
+  /// Return a list of valid loadable components in a given package.
+  std::vector<ComponentResource>
+  get_component_resources(const std::string & package_name) const;
+
+  std::shared_ptr<rclcpp_components::NodeFactory>
+  create_component_factory(const ComponentResource & resource);
+
+private:
+  void
+  OnLoadNode(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<LoadNode::Request> request,
+    std::shared_ptr<LoadNode::Response> response);
+
+  void
+  OnUnloadNode(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<UnloadNode::Request> request,
+    std::shared_ptr<UnloadNode::Response> response);
+
+  void
+  OnListNodes(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<ListNodes::Request> request,
+    std::shared_ptr<ListNodes::Response> response);
+
+private:
+  std::weak_ptr<rclcpp::executor::Executor> executor_;
+
+  uint64_t unique_id {1};
+  std::map<std::string, std::unique_ptr<class_loader::ClassLoader>> loaders_;
+  std::map<uint64_t, rclcpp_components::NodeInstanceWrapper> node_wrappers_;
+
+  rclcpp::Service<LoadNode>::SharedPtr loadNode_srv_;
+  rclcpp::Service<UnloadNode>::SharedPtr unloadNode_srv_;
+  rclcpp::Service<ListNodes>::SharedPtr listNodes_srv_;
+};
+
+}  // namespace rclcpp_components
+
+#endif  // COMPONENT_MANAGER_HPP__

rclcpp_components/test/components/test_component.cpp

@@ -0,0 +1,68 @@
+// 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/rclcpp.hpp"
+
+namespace test_rclcpp_components
+{
+/// Simple test component
+class TestComponentFoo : public rclcpp::Node
+{
+public:
+  explicit TestComponentFoo(rclcpp::NodeOptions options)
+  : rclcpp::Node("test_component_foo", options)
+  {
+  }
+};
+
+/// Simple test component
+class TestComponentBar : public rclcpp::Node
+{
+public:
+  explicit TestComponentBar(rclcpp::NodeOptions options)
+  : rclcpp::Node("test_component_bar", options)
+  {
+  }
+};
+
+/// Simple test component that doesn't inherit from rclcpp::Node
+class TestComponentNoNode
+{
+public:
+  explicit TestComponentNoNode(rclcpp::NodeOptions options)
+  : node_("test_component_no_node", options)
+  {
+  }
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  get_node_base_interface()
+  {
+    return node_.get_node_base_interface();
+  }
+
+private:
+  rclcpp::Node node_;
+};
+
+
+}  // namespace test_rclcpp_components
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentFoo)
+RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentBar)
+RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentNoNode)

rclcpp_components/test/test_component_manager.cpp

@@ -0,0 +1,89 @@
+// 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 <memory>
+
+#include "component_manager.hpp"
+
+#include "rcpputils/filesystem_helper.hpp"
+
+class TestComponentManager : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+TEST_F(TestComponentManager, get_component_resources_invalid)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  EXPECT_THROW(manager->get_component_resources("invalid_package"),
+    rclcpp_components::ComponentManagerException);
+}
+
+TEST_F(TestComponentManager, get_component_resources_valid)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  auto resources = manager->get_component_resources("rclcpp_components");
+  EXPECT_EQ(3u, resources.size());
+
+  EXPECT_EQ("test_rclcpp_components::TestComponentFoo", resources[0].first);
+  EXPECT_EQ("test_rclcpp_components::TestComponentBar", resources[1].first);
+  EXPECT_EQ("test_rclcpp_components::TestComponentNoNode", resources[2].first);
+
+  EXPECT_TRUE(rcpputils::fs::path(resources[0].second).exists());
+  EXPECT_TRUE(rcpputils::fs::path(resources[1].second).exists());
+  EXPECT_TRUE(rcpputils::fs::path(resources[2].second).exists());
+}
+
+TEST_F(TestComponentManager, create_component_factory_valid)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  auto resources = manager->get_component_resources("rclcpp_components");
+  EXPECT_EQ(3u, resources.size());
+
+  // Repeated loading should reuse existing class loader and not throw.
+  EXPECT_NO_THROW(auto factory = manager->create_component_factory(resources[0]););
+  EXPECT_NO_THROW(auto factory = manager->create_component_factory(resources[0]););
+
+  for (const auto & resource : resources) {
+    auto factory = manager->create_component_factory(resource);
+    EXPECT_NE(nullptr, factory);
+  }
+}
+
+TEST_F(TestComponentManager, create_component_factory_invalid)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  // Test invalid library
+  EXPECT_THROW(manager->create_component_factory({"foo_class", "invalid_library.so"}),
+    rclcpp_components::ComponentManagerException);
+
+  // Test valid library with invalid class
+  auto resources = manager->get_component_resources("rclcpp_components");
+  auto factory = manager->create_component_factory({"foo_class", resources[0].second});
+  EXPECT_EQ(nullptr, factory);
+}