0.5.0

* Further expand test tolerance to address flakiness.

* Remove newline.

.github/ISSUE_TEMPLATE.md

@@ -1,5 +1,6 @@
 <!--
-For general questions, please post on discourse: https://discourse.ros.org/c/ng-ros
+For general questions, please ask on ROS answers: https://answers.ros.org, make sure to include at least the `ros2` tag and the rosdistro version you are running, e.g. `ardent`.
+For general design discussions, please post on discourse: https://discourse.ros.org/c/ng-ros
 Not sure if this is the right repository? Open an issue on https://github.com/ros2/ros2/issues
 For Bug report or feature requests, please fill out the relevant category below
 -->

rclcpp/CHANGELOG.rst

@@ -0,0 +1,46 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rclcpp
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.0 (2018-06-25)
+------------------
+* Fixed a bug in the multi-threaded executor which could cause it to take a timer (potentially other types of wait-able items) more than once to be worked one. (`#383 <https://github.com/ros2/rclcpp/issues/383>`_)
+  * Specifically this could result in a timer getting called more often that it should when using the multi-threaded executor.
+* Added functions that allow you to publish serialized messages and received serialized messages in your subscription callback. (`#388 <https://github.com/ros2/rclcpp/issues/388>`_)
+* Changed code to always get the Service name from ``rcl`` to ensure the remapped name is returned. (`#498 <https://github.com/ros2/rclcpp/issues/498>`_)
+* Added previously missing ``set_parameters_atomically()`` method to the Service client interface. (`#494 <https://github.com/ros2/rclcpp/issues/494>`_)
+* Added ability to initialize parameter values in a Node via a YAML file passed on the command line. (`#488 <https://github.com/ros2/rclcpp/issues/488>`_)
+* Fixed the ROS parameter interface which got parameters that aren't set. (`#493 <https://github.com/ros2/rclcpp/issues/493>`_)
+* Added ability to initialize parameter values in a node with an argument to the Node constructor. (`#486 <https://github.com/ros2/rclcpp/issues/486>`_)
+* Added a ``Subscription`` tests which uses ``std::bind`` to a class member callback. (`#480 <https://github.com/ros2/rclcpp/issues/480>`_)
+* Refactored the ``ParameterVariant`` class into the ``Parameter`` and ``ParameterValue`` classes. (`#481 <https://github.com/ros2/rclcpp/issues/481>`_)
+* Relaxed template matching rules for ``std::bind`` and ``GNU C++ >= 7.1``. (`#484 <https://github.com/ros2/rclcpp/issues/484>`_)
+* Changed to use the new ``rosgraph_msgs/Clock`` message type for the ``/clock`` topic. (`#474 <https://github.com/ros2/rclcpp/issues/474>`_)
+* Fixed a flaky ROS time test due to not spinning before getting the time. (`#483 <https://github.com/ros2/rclcpp/issues/483>`_)
+* Nodes now autostart the ROS parameter services which let you get, set, and list parameters in a node. (`#478 <https://github.com/ros2/rclcpp/issues/478>`_)
+* Added support for arrays in Parameters. (`#443 <https://github.com/ros2/rclcpp/issues/443>`_)
+* Changed how executors use ``AnyExecutable`` objects so that they are a reference instead of a shared pointer, in order to avoid memory allocation in the "common case". (`#463 <https://github.com/ros2/rclcpp/issues/463>`_)
+* Added ability to pass command line arguments to the Node constructor. (`#461 <https://github.com/ros2/rclcpp/issues/461>`_)
+* Added an argument to specify the number of threads a multithreaded executor should create. (`#442 <https://github.com/ros2/rclcpp/issues/442>`_)
+* Changed library export order for static linking. (`#446 <https://github.com/ros2/rclcpp/issues/446>`_)
+* Fixed some typos in the time unit tests. (`#453 <https://github.com/ros2/rclcpp/issues/453>`_)
+  Obviously it mean RCL_SYSTEM_TIME but not RCL_ROS_TIME in some test cases
+  * Signed-off-by: jwang <jing.j.wang@intel.com>
+* Added the scale operation to ``rclcpp::Duration``.
+  * Signed-off-by: jwang <jing.j.wang@intel.com>
+* Changed API of the log location parameter to be ``const``. (`#451 <https://github.com/ros2/rclcpp/issues/451>`_)
+* Changed how the subscriber, client, service, and timer handles are stored to resolve shutdown order issues. (`#431 <https://github.com/ros2/rclcpp/issues/431>`_ and `#448 <https://github.com/ros2/rclcpp/issues/448>`_)
+* Updated to get the node's logger name from ``rcl``. (`#433 <https://github.com/ros2/rclcpp/issues/433>`_)
+* Now depends on ``ament_cmake_ros``. (`#444 <https://github.com/ros2/rclcpp/issues/444>`_)
+* Updaed code to use logging macros rather than ``fprintf()``. (`#439 <https://github.com/ros2/rclcpp/issues/439>`_)
+* Fixed a bug that was using an invalid iterator when erasing items using an iterator in a loop. (`#436 <https://github.com/ros2/rclcpp/issues/436>`_)
+* Changed code to support move of ``rcutils_time_point_value_t`` type from ``uint64_t`` to ``int64_t``. (`#429 <https://github.com/ros2/rclcpp/issues/429>`_)
+* Renamed parameter byte type to ``byte_values`` from ``bytes_value``. (`#428 <https://github.com/ros2/rclcpp/issues/428>`_)
+* Changed executor code to clear the wait set before resizing and waiting. (`#427 <https://github.com/ros2/rclcpp/issues/427>`_)
+* Fixed a potential dereference of nullptr in the topic name validation error string. (`#405 <https://github.com/ros2/rclcpp/issues/405>`_)
+  * Signed-off-by: Ethan Gao <ethan.gao@linux.intel.com>
+* Changed to use ``rcl_count_publishers()`` like API's rather than the lower level ``rmw_count_publishers()`` API. (`#425 <https://github.com/ros2/rclcpp/issues/425>`_)
+  * Signed-off-by: Sriram Raghunathan <rsriram7@visteon.com>
+* Fix potential segmentation fault due to ``get_topic_name()`` or ``rcl_service_get_service_name()`` returning nullptr and that not being checked before access in ``rclcpp``. (`#426 <https://github.com/ros2/rclcpp/issues/426>`_)
+  * Signed-off-by: Ethan Gao <ethan.gao@linux.intel.com>
+* Contributors: Denise Eng, Dirk Thomas, Ernesto Corbellini, Esteve Fernandez, Ethan Gao, Guillaume Autran, Karsten Knese, Matthew, Michael Carroll, Mikael Arguedas, Shane Loretz, Sriram Raghunathan, Tom Moore, William Woodall, dhood, jwang, jwang11, serge-nikulin

rclcpp/CMakeLists.txt

@@ -2,12 +2,14 @@ cmake_minimum_required(VERSION 3.5)
 
 project(rclcpp)
 
-find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
+find_package(rcl_yaml_param_parser REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
+find_package(rosgraph_msgs REQUIRED)
 find_package(rosidl_generator_cpp REQUIRED)
 find_package(rosidl_typesupport_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
@@ -58,8 +60,10 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/node_interfaces/node_timers.cpp
   src/rclcpp/node_interfaces/node_topics.cpp
   src/rclcpp/parameter.cpp
+  src/rclcpp/parameter_value.cpp
   src/rclcpp/parameter_client.cpp
   src/rclcpp/parameter_events_filter.cpp
+  src/rclcpp/parameter_map.cpp
   src/rclcpp/parameter_service.cpp
   src/rclcpp/publisher.cpp
   src/rclcpp/service.cpp
@@ -93,13 +97,16 @@ list(APPEND ${PROJECT_NAME}_SRCS
   include/rclcpp/logging.hpp)
 include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")
 
-add_library(${PROJECT_NAME} SHARED
+add_library(${PROJECT_NAME}
   ${${PROJECT_NAME}_SRCS})
+# specific order: dependents before dependencies
 ament_target_dependencies(${PROJECT_NAME}
-  "builtin_interfaces"
   "rcl"
-  "rosidl_generator_cpp"
-  "rosidl_typesupport_cpp")
+  "rcl_yaml_param_parser"
+  "builtin_interfaces"
+  "rosgraph_msgs"
+  "rosidl_typesupport_cpp"
+  "rosidl_generator_cpp")
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
@@ -113,18 +120,21 @@ install(
   RUNTIME DESTINATION bin
 )
 
-ament_export_dependencies(ament_cmake)
-ament_export_dependencies(builtin_interfaces)
-ament_export_dependencies(rcl)
-ament_export_dependencies(rosidl_generator_cpp)
-ament_export_dependencies(rosidl_typesupport_c)
-ament_export_dependencies(rosidl_typesupport_cpp)
-
+# specific order: dependents before dependencies
 ament_export_include_directories(include)
-
 ament_export_libraries(${PROJECT_NAME})
 
+ament_export_dependencies(ament_cmake)
+ament_export_dependencies(rcl)
+ament_export_dependencies(builtin_interfaces)
+ament_export_dependencies(rosgraph_msgs)
+ament_export_dependencies(rosidl_typesupport_cpp)
+ament_export_dependencies(rosidl_typesupport_c)
+ament_export_dependencies(rosidl_generator_cpp)
+ament_export_dependencies(rcl_yaml_param_parser)
+
 if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
@@ -189,6 +199,20 @@ if(BUILD_TESTING)
     )
     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}
+    )
+    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
@@ -199,6 +223,20 @@ if(BUILD_TESTING)
     )
     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}
+    )
+    target_link_libraries(test_parameter ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_parameter_map test/test_parameter_map.cpp)
+  if(TARGET test_parameter_map)
+    target_link_libraries(test_parameter_map ${PROJECT_NAME})
+  endif()
   ament_add_gtest(test_publisher test/test_publisher.cpp)
   if(TARGET test_publisher)
     target_include_directories(test_publisher PUBLIC
@@ -222,6 +260,17 @@ if(BUILD_TESTING)
       ${PROJECT_NAME}
     )
   endif()
+  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp
+    ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
+  if(TARGET test_serialized_message_allocator)
+    target_include_directories(test_serialized_message_allocator PUBLIC
+      ${test_msgs_INCLUDE_DIRS}
+    )
+    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
@@ -242,6 +291,16 @@ if(BUILD_TESTING)
     )
     target_link_libraries(test_subscription ${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
+    "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
@@ -320,6 +379,22 @@ if(BUILD_TESTING)
       "rcl")
     target_link_libraries(test_time_source ${PROJECT_NAME})
   endif()
+
+  ament_add_gtest(test_utilities test/test_utilities.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_utilities)
+    ament_target_dependencies(test_utilities
+      "rcl")
+    target_link_libraries(test_utilities ${PROJECT_NAME})
+  endif()
+
+  ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_multi_threaded_executor)
+    ament_target_dependencies(test_multi_threaded_executor
+      "rcl")
+    target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_package(

rclcpp/include/rclcpp/allocator/allocator_deleter.hpp

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

rclcpp/include/rclcpp/any_executable.hpp

@@ -33,8 +33,6 @@ namespace executor
 
 struct AnyExecutable
 {
-  RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable)
-
   RCLCPP_PUBLIC
   AnyExecutable();
 

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -32,16 +32,16 @@ class AnyServiceCallback
 {
 private:
   using SharedPtrCallback = std::function<
-      void(
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>
-      )>;
+    void(
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
   using SharedPtrWithRequestHeaderCallback = std::function<
-      void(
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>
-      )>;
+    void(
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>
+    )>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_;

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -40,13 +40,13 @@ class AnySubscriptionCallback
 
   using SharedPtrCallback = std::function<void(const std::shared_ptr<MessageT>)>;
   using SharedPtrWithInfoCallback =
-      std::function<void(const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
+    std::function<void(const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
   using ConstSharedPtrCallback = std::function<void(const std::shared_ptr<const MessageT>)>;
   using ConstSharedPtrWithInfoCallback =
-      std::function<void(const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
+    std::function<void(const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
   using UniquePtrCallback = std::function<void(MessageUniquePtr)>;
   using UniquePtrWithInfoCallback =
-      std::function<void(MessageUniquePtr, const rmw_message_info_t &)>;
+    std::function<void(MessageUniquePtr, const rmw_message_info_t &)>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback_;

rclcpp/include/rclcpp/client.hpp

@@ -36,6 +36,8 @@
 #include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcutils/logging_macros.h"
+
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -55,22 +57,21 @@ public:
   RCLCPP_PUBLIC
   ClientBase(
     rclcpp::node_interfaces::NodeBaseInterface * node_base,
-    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
-    const std::string & service_name);
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph);
 
   RCLCPP_PUBLIC
   virtual ~ClientBase();
 
   RCLCPP_PUBLIC
-  const std::string &
+  const char *
   get_service_name() const;
 
   RCLCPP_PUBLIC
-  rcl_client_t *
+  std::shared_ptr<rcl_client_t>
   get_client_handle();
 
   RCLCPP_PUBLIC
-  const rcl_client_t *
+  std::shared_ptr<const rcl_client_t>
   get_client_handle() const;
 
   RCLCPP_PUBLIC
@@ -110,8 +111,7 @@ protected:
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  rcl_client_t client_handle_ = rcl_get_zero_initialized_client();
-  std::string service_name_;
+  std::shared_ptr<rcl_client_t> client_handle_;
 };
 
 template<typename ServiceT>
@@ -140,13 +140,13 @@ public:
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
     const std::string & service_name,
     rcl_client_options_t & client_options)
-  : ClientBase(node_base, node_graph, service_name)
+  : ClientBase(node_base, node_graph)
   {
     using rosidl_typesupport_cpp::get_service_type_support_handle;
     auto service_type_support_handle =
       get_service_type_support_handle<ServiceT>();
     rcl_ret_t ret = rcl_client_init(
-      &client_handle_,
+      this->get_client_handle().get(),
       this->get_rcl_node_handle(),
       service_type_support_handle,
       service_name.c_str(),
@@ -168,11 +168,6 @@ public:
 
   virtual ~Client()
   {
-    if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) {
-      fprintf(stderr,
-        "Error in destruction of rcl client handle: %s\n", rcl_get_error_string_safe());
-      rcl_reset_error();
-    }
   }
 
   std::shared_ptr<void>
@@ -199,7 +194,9 @@ public:
     int64_t sequence_number = request_header->sequence_number;
     // TODO(esteve) this should throw instead since it is not expected to happen in the first place
     if (this->pending_requests_.count(sequence_number) == 0) {
-      fprintf(stderr, "Received invalid sequence number. Ignoring...\n");
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "Received invalid sequence number. Ignoring...");
       return;
     }
     auto tuple = this->pending_requests_[sequence_number];
@@ -234,7 +231,7 @@ public:
   {
     std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     int64_t sequence_number;
-    rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number);
+    rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);
     if (RCL_RET_OK != ret) {
       rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }

rclcpp/include/rclcpp/create_service.hpp

@@ -0,0 +1,58 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__CREATE_SERVICE_HPP_
+#define RCLCPP__CREATE_SERVICE_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rmw/rmw.h"
+
+namespace rclcpp
+{
+
+/// Create a service with a given type.
+/// \internal
+template<typename ServiceT, typename CallbackT>
+typename rclcpp::Service<ServiceT>::SharedPtr
+create_service(
+  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  const std::string & service_name,
+  CallbackT && callback,
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
+  any_service_callback.set(std::forward<CallbackT>(callback));
+
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  service_options.qos = qos_profile;
+
+  auto serv = Service<ServiceT>::make_shared(
+    node_base->get_shared_rcl_node_handle(),
+    service_name, any_service_callback, service_options);
+  auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
+  node_services->add_service(serv_base_ptr, group);
+  return serv;
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CREATE_SERVICE_HPP_

rclcpp/include/rclcpp/create_subscription.hpp

@@ -26,8 +26,13 @@
 namespace rclcpp
 {
 
-template<typename MessageT, typename CallbackT, typename AllocatorT, typename SubscriptionT>
-typename rclcpp::Subscription<MessageT, AllocatorT>::SharedPtr
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT,
+  typename CallbackMessageT,
+  typename SubscriptionT = rclcpp::Subscription<CallbackMessageT, AllocatorT>>
+typename std::shared_ptr<SubscriptionT>
 create_subscription(
   rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
   const std::string & topic_name,
@@ -36,7 +41,8 @@ create_subscription(
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
   bool use_intra_process_comms,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, AllocatorT>::SharedPtr
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT, AllocatorT>::SharedPtr
   msg_mem_strat,
   typename std::shared_ptr<AllocatorT> allocator)
 {
@@ -44,8 +50,8 @@ create_subscription(
   subscription_options.qos = qos_profile;
   subscription_options.ignore_local_publications = ignore_local_publications;
 
-  auto factory =
-    rclcpp::create_subscription_factory<MessageT, CallbackT, AllocatorT, SubscriptionT>(
+  auto factory = rclcpp::create_subscription_factory
+    <MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT>(
     std::forward<CallbackT>(callback), msg_mem_strat, allocator);
 
   auto sub = node_topics->create_subscription(

rclcpp/include/rclcpp/duration.hpp

@@ -89,6 +89,10 @@ public:
   Duration
   operator-(const rclcpp::Duration & rhs) const;
 
+  RCLCPP_PUBLIC
+  Duration
+  operator*(double scale) const;
+
   RCLCPP_PUBLIC
   rcl_duration_value_t
   nanoseconds() const;

rclcpp/include/rclcpp/exceptions.hpp

@@ -181,6 +181,21 @@ public:
   : std::runtime_error("event already registered") {}
 };
 
+/// Thrown if passed parameters are inconsistent or invalid
+class InvalidParametersException : public std::runtime_error
+{
+public:
+  // Inherit constructors from runtime_error;
+  using std::runtime_error::runtime_error;
+};
+
+/// Throwing if passed parameter value is invalid.
+class InvalidParameterValueException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error;
+  using std::runtime_error::runtime_error;
+};
+
 }  // namespace exceptions
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/executor.hpp

@@ -287,7 +287,7 @@ protected:
    */
   RCLCPP_PUBLIC
   void
-  execute_any_executable(AnyExecutable::SharedPtr any_exec);
+  execute_any_executable(AnyExecutable & any_exec);
 
   RCLCPP_PUBLIC
   static void
@@ -325,15 +325,17 @@ protected:
 
   RCLCPP_PUBLIC
   void
-  get_next_timer(AnyExecutable::SharedPtr any_exec);
+  get_next_timer(AnyExecutable & any_exec);
 
   RCLCPP_PUBLIC
-  AnyExecutable::SharedPtr
-  get_next_ready_executable();
+  bool
+  get_next_ready_executable(AnyExecutable & any_executable);
 
   RCLCPP_PUBLIC
-  AnyExecutable::SharedPtr
-  get_next_executable(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+  bool
+  get_next_executable(
+    AnyExecutable & any_executable,
+    std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
   /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins.
   std::atomic_bool spinning;

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -15,7 +15,9 @@
 #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
 
+#include <memory>
 #include <mutex>
+#include <set>
 #include <thread>
 #include <unordered_map>
 
@@ -34,9 +36,22 @@ class MultiThreadedExecutor : public executor::Executor
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor)
 
+  /// Constructor for MultiThreadedExecutor.
+  /**
+   * For the yield_before_execute option, when true std::this_thread::yield()
+   * will be called after acquiring work (as an AnyExecutable) and
+   * releasing the spinning lock, but before executing the work.
+   * This is useful for reproducing some bugs related to taking work more than
+   * once.
+   *
+   * \param args common arguments for all executors
+   * \param yield_before_execute if true std::this_thread::yield() is called
+   */
   RCLCPP_PUBLIC
   MultiThreadedExecutor(
-    const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments());
+    const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments(),
+    size_t number_of_threads = 0,
+    bool yield_before_execute = false);
 
   RCLCPP_PUBLIC
   virtual ~MultiThreadedExecutor();
@@ -59,6 +74,10 @@ private:
 
   std::mutex wait_mutex_;
   size_t number_of_threads_;
+  bool yield_before_execute_;
+
+  std::mutex scheduled_timers_mutex_;
+  std::set<TimerBase::SharedPtr> scheduled_timers_;
 };
 
 }  // namespace executors

rclcpp/include/rclcpp/function_traits.hpp

@@ -49,7 +49,7 @@ template<typename FunctionT>
 struct function_traits
 {
   using arguments = typename tuple_tail<
-      typename function_traits<decltype( & FunctionT::operator())>::arguments>::type;
+    typename function_traits<decltype( & FunctionT::operator())>::arguments>::type;
 
   static constexpr std::size_t arity = std::tuple_size<arguments>::value;
 
@@ -83,7 +83,7 @@ template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ...
 #if defined _LIBCPP_VERSION  // libc++ (Clang)
 struct function_traits<std::__1::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
 #elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
-struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(ClassT *, FArgs ...))(Args ...)>>
+struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...)>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<std::_Mem_fn<ReturnTypeT (ClassT::*)(Args ...)>(FArgs ...)>>
 #elif defined _MSC_VER  // MS Visual Studio

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

@@ -258,9 +258,9 @@ private:
 
   using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
   using SubscriptionMap = std::unordered_map<
-      uint64_t, SubscriptionBase::WeakPtr,
-      std::hash<uint64_t>, std::equal_to<uint64_t>,
-      RebindAlloc<std::pair<const uint64_t, SubscriptionBase::WeakPtr>>>;
+    uint64_t, SubscriptionBase::WeakPtr,
+    std::hash<uint64_t>, std::equal_to<uint64_t>,
+    RebindAlloc<std::pair<const uint64_t, SubscriptionBase::WeakPtr>>>;
 
   struct strcmp_wrapper : public std::binary_function<const char *, const char *, bool>
   {
@@ -271,10 +271,10 @@ private:
     }
   };
   using IDTopicMap = std::map<
-      const char *,
-      AllocSet,
-      strcmp_wrapper,
-      RebindAlloc<std::pair<const char * const, AllocSet>>>;
+    const char *,
+    AllocSet,
+    strcmp_wrapper,
+    RebindAlloc<std::pair<const char * const, AllocSet>>>;
 
   SubscriptionMap subscriptions_;
 
@@ -291,16 +291,16 @@ private:
     mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
 
     using TargetSubscriptionsMap = std::unordered_map<
-        uint64_t, AllocSet,
-        std::hash<uint64_t>, std::equal_to<uint64_t>,
-        RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
+      uint64_t, AllocSet,
+      std::hash<uint64_t>, std::equal_to<uint64_t>,
+      RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
     TargetSubscriptionsMap target_subscriptions_by_message_sequence;
   };
 
   using PublisherMap = std::unordered_map<
-      uint64_t, PublisherInfo,
-      std::hash<uint64_t>, std::equal_to<uint64_t>,
-      RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
+    uint64_t, PublisherInfo,
+    std::hash<uint64_t>, std::equal_to<uint64_t>,
+    RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
 
   PublisherMap publishers_;
 

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -55,27 +55,23 @@ public:
   virtual void clear_handles() = 0;
   virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0;
 
-  /// Provide a newly initialized AnyExecutable object.
-  // \return Shared pointer to the fresh executable.
-  virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
-
   virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
   virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0;
 
   virtual void
   get_next_subscription(
-    rclcpp::executor::AnyExecutable::SharedPtr any_exec,
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual void
   get_next_service(
-    rclcpp::executor::AnyExecutable::SharedPtr any_exec,
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual void
   get_next_client(
-    rclcpp::executor::AnyExecutable::SharedPtr any_exec,
+    rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
   virtual rcl_allocator_t
@@ -83,14 +79,18 @@ public:
 
   static rclcpp::SubscriptionBase::SharedPtr
   get_subscription_by_handle(
-    const rcl_subscription_t * subscriber_handle,
+    std::shared_ptr<const rcl_subscription_t> subscriber_handle,
     const WeakNodeVector & weak_nodes);
 
   static rclcpp::ServiceBase::SharedPtr
-  get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes);
+  get_service_by_handle(
+    std::shared_ptr<const rcl_service_t> service_handle,
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::ClientBase::SharedPtr
-  get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes);
+  get_client_by_handle(
+    std::shared_ptr<const rcl_client_t> client_handle,
+    const WeakNodeVector & weak_nodes);
 
   static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
   get_node_by_group(

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -18,10 +18,15 @@
 #include <memory>
 #include <stdexcept>
 
+#include "rcl/types.h"
+
 #include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rmw/serialized_message.h"
+
 namespace rclcpp
 {
 namespace message_memory_strategy
@@ -39,14 +44,29 @@ public:
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
 
+  using SerializedMessageAllocTraits = allocator::AllocRebind<rcl_serialized_message_t, Alloc>;
+  using SerializedMessageAlloc = typename SerializedMessageAllocTraits::allocator_type;
+  using SerializedMessageDeleter =
+    allocator::Deleter<SerializedMessageAlloc, rcl_serialized_message_t>;
+
+  using BufferAllocTraits = allocator::AllocRebind<char, Alloc>;
+  using BufferAlloc = typename BufferAllocTraits::allocator_type;
+  using BufferDeleter = allocator::Deleter<BufferAlloc, char>;
+
   MessageMemoryStrategy()
   {
     message_allocator_ = std::make_shared<MessageAlloc>();
+    serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>();
+    buffer_allocator_ = std::make_shared<BufferAlloc>();
+    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
   }
 
   explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>(*allocator.get());
+    buffer_allocator_ = std::make_shared<BufferAlloc>(*allocator.get());
+    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
   }
 
   /// Default factory method
@@ -62,6 +82,37 @@ public:
     return std::allocate_shared<MessageT, MessageAlloc>(*message_allocator_.get());
   }
 
+  virtual std::shared_ptr<rcl_serialized_message_t> borrow_serialized_message(size_t capacity)
+  {
+    auto msg = new rcl_serialized_message_t;
+    *msg = rmw_get_zero_initialized_serialized_message();
+    auto ret = rmw_serialized_message_init(msg, capacity, &rcutils_allocator_);
+    if (ret != RCL_RET_OK) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+
+    auto serialized_msg = std::shared_ptr<rcl_serialized_message_t>(msg,
+        [](rmw_serialized_message_t * msg) {
+          auto ret = rmw_serialized_message_fini(msg);
+          delete msg;
+          if (ret != RCL_RET_OK) {
+            rclcpp::exceptions::throw_from_rcl_error(ret, "leaking memory");
+          }
+        });
+
+    return serialized_msg;
+  }
+
+  virtual std::shared_ptr<rcl_serialized_message_t> borrow_serialized_message()
+  {
+    return borrow_serialized_message(default_buffer_capacity_);
+  }
+
+  virtual void set_default_buffer_capacity(size_t capacity)
+  {
+    default_buffer_capacity_ = capacity;
+  }
+
   /// Release ownership of the message, which will deallocate it if it has no more owners.
   /** \param[in] msg Shared pointer to the message we are returning. */
   virtual void return_message(std::shared_ptr<MessageT> & msg)
@@ -69,8 +120,22 @@ public:
     msg.reset();
   }
 
+  virtual void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & serialized_msg)
+  {
+    serialized_msg.reset();
+  }
+
   std::shared_ptr<MessageAlloc> message_allocator_;
   MessageDeleter message_deleter_;
+
+  std::shared_ptr<SerializedMessageAlloc> serialized_message_allocator_;
+  SerializedMessageDeleter serialized_message_deleter_;
+
+  std::shared_ptr<BufferAlloc> buffer_allocator_;
+  BufferDeleter buffer_deleter_;
+  size_t default_buffer_capacity_ = 0;
+
+  rcutils_allocator_t rcutils_allocator_;
 };
 
 }  // namespace message_memory_strategy

rclcpp/include/rclcpp/node.hpp

@@ -53,6 +53,7 @@
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/service.hpp"
 #include "rclcpp/subscription.hpp"
+#include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -84,6 +85,10 @@ public:
    * \param[in] node_name Name of the node.
    * \param[in] namespace_ Namespace of the node.
    * \param[in] context The context for the node (usually represents the state of a process).
+   * \param[in] arguments Command line arguments that should apply only to this node.
+   * \param[in] initial_parameters a list of initial values for parameters on the node.
+   * This can be used to provide remapping rules that only affect one instance.
+   * \param[in] use_global_arguments False to prevent node using arguments passed to the process.
    * \param[in] use_intra_process_comms True to use the optimized intra-process communication
    * pipeline to pass messages between nodes in the same process using shared memory.
    */
@@ -92,7 +97,11 @@ public:
     const std::string & node_name,
     const std::string & namespace_,
     rclcpp::Context::SharedPtr context,
-    bool use_intra_process_comms = false);
+    const std::vector<std::string> & arguments,
+    const std::vector<Parameter> & initial_parameters,
+    bool use_global_arguments = true,
+    bool use_intra_process_comms = false,
+    bool start_parameter_services = true);
 
   RCLCPP_PUBLIC
   virtual ~Node();
@@ -175,7 +184,8 @@ public:
     typename MessageT,
     typename CallbackT,
     typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<MessageT, Alloc>>
+    typename SubscriptionT = rclcpp::Subscription<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>>
   std::shared_ptr<SubscriptionT>
   create_subscription(
     const std::string & topic_name,
@@ -183,7 +193,8 @@ public:
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
     bool ignore_local_publications = false,
-    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
     msg_mem_strat = nullptr,
     std::shared_ptr<Alloc> allocator = nullptr);
 
@@ -206,15 +217,17 @@ public:
     typename MessageT,
     typename CallbackT,
     typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<MessageT, Alloc>>
+    typename SubscriptionT = rclcpp::Subscription<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>>
   std::shared_ptr<SubscriptionT>
   create_subscription(
     const std::string & topic_name,
-    size_t qos_history_depth,
     CallbackT && callback,
+    size_t qos_history_depth,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
     bool ignore_local_publications = false,
-    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
     msg_mem_strat = nullptr,
     std::shared_ptr<Alloc> allocator = nullptr);
 
@@ -250,11 +263,11 @@ public:
 
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_PUBLIC
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
 
   template<typename ParameterT>
   void
@@ -263,18 +276,18 @@ public:
     const ParameterT & value);
 
   RCLCPP_PUBLIC
-  std::vector<rclcpp::parameter::ParameterVariant>
+  std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & names) const;
 
   RCLCPP_PUBLIC
-  rclcpp::parameter::ParameterVariant
+  rclcpp::Parameter
   get_parameter(const std::string & name) const;
 
   RCLCPP_PUBLIC
   bool
   get_parameter(
     const std::string & name,
-    rclcpp::parameter::ParameterVariant & parameter) const;
+    rclcpp::Parameter & parameter) const;
 
   /// Assign the value of the parameter if set into the parameter argument.
   /**
@@ -346,7 +359,7 @@ public:
 
   /// Return a graph event, which will be set anytime a graph change occurs.
   /* The graph Event object is a loan which must be returned.
-   * The Event object is scoped and therefore to return the load just let it go
+   * The Event object is scoped and therefore to return the loan just let it go
    * out of scope.
    */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_impl.hpp

@@ -39,6 +39,7 @@
 #include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/create_publisher.hpp"
+#include "rclcpp/create_service.hpp"
 #include "rclcpp/create_subscription.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -81,7 +82,11 @@ Node::create_publisher(
     allocator);
 }
 
-template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename Alloc,
+  typename SubscriptionT>
 std::shared_ptr<SubscriptionT>
 Node::create_subscription(
   const std::string & topic_name,
@@ -89,20 +94,23 @@ Node::create_subscription(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  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)
 {
+  using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
+
   if (!allocator) {
     allocator = std::make_shared<Alloc>();
   }
 
   if (!msg_mem_strat) {
     using rclcpp::message_memory_strategy::MessageMemoryStrategy;
-    msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
+    msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, Alloc>::create_default();
   }
 
-  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
+  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, CallbackMessageT, SubscriptionT>(
     this->node_topics_.get(),
     topic_name,
     std::forward<CallbackT>(callback),
@@ -114,21 +122,26 @@ Node::create_subscription(
     allocator);
 }
 
-template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename Alloc,
+  typename SubscriptionT>
 std::shared_ptr<SubscriptionT>
 Node::create_subscription(
   const std::string & topic_name,
-  size_t qos_history_depth,
   CallbackT && callback,
+  size_t qos_history_depth,
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  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)
 {
   rmw_qos_profile_t qos = rmw_qos_profile_default;
   qos.depth = qos_history_depth;
-  return this->create_subscription<MessageT, CallbackT, Alloc, SubscriptionT>(
+  return this->create_subscription<MessageT>(
     topic_name,
     std::forward<CallbackT>(callback),
     qos,
@@ -184,18 +197,9 @@ Node::create_service(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
-  any_service_callback.set(std::forward<CallbackT>(callback));
-
-  rcl_service_options_t service_options = rcl_service_get_default_options();
-  service_options.qos = qos_profile;
-
-  auto serv = Service<ServiceT>::make_shared(
-    node_base_->get_shared_rcl_node_handle(),
-    service_name, any_service_callback, service_options);
-  auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
-  node_services_->add_service(serv_base_ptr, group);
-  return serv;
+  return rclcpp::create_service<ServiceT, CallbackT>(
+    node_base_, node_services_,
+    service_name, std::forward<CallbackT>(callback), qos_profile, group);
 }
 
 template<typename CallbackT>
@@ -211,10 +215,10 @@ Node::set_parameter_if_not_set(
   const std::string & name,
   const ParameterT & value)
 {
-  rclcpp::parameter::ParameterVariant parameter_variant;
-  if (!this->get_parameter(name, parameter_variant)) {
+  rclcpp::Parameter parameter;
+  if (!this->get_parameter(name, parameter)) {
     this->set_parameters({
-        rclcpp::parameter::ParameterVariant(name, value),
+        rclcpp::Parameter(name, value),
       });
   }
 }
@@ -223,10 +227,10 @@ template<typename ParameterT>
 bool
 Node::get_parameter(const std::string & name, ParameterT & value) const
 {
-  rclcpp::parameter::ParameterVariant parameter_variant;
-  bool result = get_parameter(name, parameter_variant);
+  rclcpp::Parameter parameter;
+  bool result = get_parameter(name, parameter);
   if (result) {
-    value = parameter_variant.get_value<ParameterT>();
+    value = parameter.get_value<ParameterT>();
   }
 
   return result;

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -39,7 +39,9 @@ public:
   NodeBase(
     const std::string & node_name,
     const std::string & namespace_,
-    rclcpp::Context::SharedPtr context);
+    rclcpp::Context::SharedPtr context,
+    const std::vector<std::string> & arguments,
+    bool use_global_arguments);
 
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_logging.hpp

@@ -46,6 +46,11 @@ public:
   rclcpp::Logger
   get_logger() const;
 
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_logger_name() const;
+
 private:
   RCLCPP_DISABLE_COPY(NodeLogging)
 

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -39,6 +39,13 @@ public:
   virtual
   rclcpp::Logger
   get_logger() const = 0;
+
+  /// Return the logger name associated with the node.
+  /** \return The logger name associated with the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_logger_name() const = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_
 
 #include <map>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -30,6 +31,7 @@
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
 #include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_service.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -46,8 +48,12 @@ public:
 
   RCLCPP_PUBLIC
   NodeParameters(
-    rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
-    bool use_intra_process);
+    const node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    const node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    const node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    const std::vector<Parameter> & initial_parameters,
+    bool use_intra_process,
+    bool start_parameter_services);
 
   RCLCPP_PUBLIC
   virtual
@@ -57,22 +63,22 @@ public:
   virtual
   std::vector<rcl_interfaces::msg::SetParametersResult>
   set_parameters(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+    const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_PUBLIC
   virtual
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+    const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_PUBLIC
   virtual
-  std::vector<rclcpp::parameter::ParameterVariant>
+  std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & names) const;
 
   RCLCPP_PUBLIC
   virtual
-  rclcpp::parameter::ParameterVariant
+  rclcpp::Parameter
   get_parameter(const std::string & name) const;
 
   RCLCPP_PUBLIC
@@ -80,7 +86,7 @@ public:
   bool
   get_parameter(
     const std::string & name,
-    rclcpp::parameter::ParameterVariant & parameter) const;
+    rclcpp::Parameter & parameter) const;
 
   RCLCPP_PUBLIC
   virtual
@@ -105,15 +111,15 @@ public:
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
 
-  rclcpp::node_interfaces::NodeTopicsInterface * node_topics_;
-
   mutable std::mutex mutex_;
 
   ParametersCallbackFunction parameters_callback_ = nullptr;
 
-  std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
+  std::map<std::string, rclcpp::Parameter> parameters_;
 
   Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
+
+  std::shared_ptr<ParameterService> parameter_service_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -41,30 +41,49 @@ public:
   virtual
   std::vector<rcl_interfaces::msg::SetParametersResult>
   set_parameters(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters) = 0;
+    const std::vector<rclcpp::Parameter> & parameters) = 0;
 
   RCLCPP_PUBLIC
   virtual
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters) = 0;
+    const std::vector<rclcpp::Parameter> & parameters) = 0;
 
+  /// Get descriptions of parameters given their names.
+  /*
+   * \param[in] names a list of parameter names to check.
+   * \return the list of parameters that were found.
+   * Any parameter not found is omitted from the returned list.
+   */
   RCLCPP_PUBLIC
   virtual
-  std::vector<rclcpp::parameter::ParameterVariant>
+  std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & names) const = 0;
 
+  /// Get the description of one parameter given a name.
+  /*
+   * \param[in] name the name of the parameter to look for.
+   * \return the parameter if it exists on the node.
+   * \throws std::out_of_range if the parameter does not exist on the node.
+   */
   RCLCPP_PUBLIC
   virtual
-  rclcpp::parameter::ParameterVariant
+  rclcpp::Parameter
   get_parameter(const std::string & name) const = 0;
 
+  /// Get the description of one parameter given a name.
+  /*
+   * \param[in] name the name of the parameter to look for.
+   * \param[out] parameter the description if parameter exists on the node.
+   * \return true if the parameter exists on the node, or
+   * \return false if the parameter does not exist.
+   */
   RCLCPP_PUBLIC
   virtual
   bool
   get_parameter(
     const std::string & name,
-    rclcpp::parameter::ParameterVariant & parameter) const = 0;
+    rclcpp::Parameter & parameter) const = 0;
 
   RCLCPP_PUBLIC
   virtual
@@ -82,8 +101,8 @@ public:
   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::ParameterVariant> &)>;
+    rcl_interfaces::msg::SetParametersResult(
+      const std::vector<rclcpp::Parameter> &)>;
 
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/parameter.hpp

@@ -15,54 +15,34 @@
 #ifndef RCLCPP__PARAMETER_HPP_
 #define RCLCPP__PARAMETER_HPP_
 
+#include <iostream>
 #include <ostream>
 #include <sstream>
 #include <string>
 #include <vector>
 
 #include "rcl_interfaces/msg/parameter.hpp"
-#include "rcl_interfaces/msg/parameter_type.hpp"
-#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rclcpp/parameter_value.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rmw/rmw.h"
 
 namespace rclcpp
 {
-namespace parameter
-{
 
-enum ParameterType
-{
-  PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
-  PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
-  PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
-  PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
-  PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
-  PARAMETER_BYTES = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES,
-};
-
-// Structure to store an arbitrary parameter with templated get/set methods
-class ParameterVariant
+/// Structure to store an arbitrary parameter with templated get/set methods.
+class Parameter
 {
 public:
   RCLCPP_PUBLIC
-  ParameterVariant();
+  Parameter();
+
   RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const bool bool_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const int int_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const int64_t int_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const float double_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const double double_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const std::string & string_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const char * string_value);
-  RCLCPP_PUBLIC
-  explicit ParameterVariant(const std::string & name, const std::vector<uint8_t> & bytes_value);
+  Parameter(const std::string & name, const ParameterValue & value);
+
+  template<typename ValueTypeT>
+  explicit Parameter(const std::string & name, ValueTypeT value)
+  : Parameter(name, ParameterValue(value))
+  {
+  }
 
   RCLCPP_PUBLIC
   ParameterType
@@ -78,105 +58,27 @@ public:
 
   RCLCPP_PUBLIC
   rcl_interfaces::msg::ParameterValue
-  get_parameter_value() const;
+  get_value_message() const;
 
-  // The following get_value() variants require the use of ParameterType
-
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
+  /// Get value of parameter using rclcpp::ParameterType as template argument.
+  template<ParameterType ParamT>
+  decltype(auto)
   get_value() const
   {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
-      // TODO(wjwwood): use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.integer_value;
+    return value_.get<ParamT>();
   }
 
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, double>::type
+  /// Get value of parameter using c++ types as template argument.
+  template<typename T>
+  decltype(auto)
   get_value() const
   {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
-      // TODO(wjwwood): use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.double_value;
+    return value_.get<T>();
   }
 
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_STRING, const std::string &>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
-      // TODO(wjwwood): use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.string_value;
-  }
-
-  template<ParameterType type>
-  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, bool>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
-      // TODO(wjwwood): use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.bool_value;
-  }
-
-  template<ParameterType type>
-  typename std::enable_if<
-    type == ParameterType::PARAMETER_BYTES, const std::vector<uint8_t> &>::type
-  get_value() const
-  {
-    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTES) {
-      // TODO(wjwwood): use custom exception
-      throw std::runtime_error("Invalid type");
-    }
-    return value_.bytes_value;
-  }
-
-  // The following get_value() variants allow the use of primitive types
-
-  template<typename type>
-  typename std::enable_if<
-    std::is_integral<type>::value && !std::is_same<type, bool>::value, int64_t>::type
-  get_value() const
-  {
-    return get_value<ParameterType::PARAMETER_INTEGER>();
-  }
-
-  template<typename type>
-  typename std::enable_if<std::is_floating_point<type>::value, double>::type
-  get_value() const
-  {
-    return get_value<ParameterType::PARAMETER_DOUBLE>();
-  }
-
-  template<typename type>
-  typename std::enable_if<std::is_convertible<type, std::string>::value, const std::string &>::type
-  get_value() const
-  {
-    return get_value<ParameterType::PARAMETER_STRING>();
-  }
-
-  template<typename type>
-  typename std::enable_if<std::is_same<type, bool>::value, bool>::type
-  get_value() const
-  {
-    return get_value<ParameterType::PARAMETER_BOOL>();
-  }
-
-  template<typename type>
-  typename std::enable_if<
-    std::is_convertible<
-      type, const std::vector<uint8_t> &>::value, const std::vector<uint8_t> &>::type
-  get_value() const
-  {
-    return get_value<ParameterType::PARAMETER_BYTES>();
-  }
+  RCLCPP_PUBLIC
+  bool
+  as_bool() const;
 
   RCLCPP_PUBLIC
   int64_t
@@ -190,21 +92,33 @@ public:
   const std::string &
   as_string() const;
 
-  RCLCPP_PUBLIC
-  bool
-  as_bool() const;
-
   RCLCPP_PUBLIC
   const std::vector<uint8_t> &
-  as_bytes() const;
+  as_byte_array() const;
 
   RCLCPP_PUBLIC
-  static ParameterVariant
-  from_parameter(const rcl_interfaces::msg::Parameter & parameter);
+  const std::vector<bool> &
+  as_bool_array() const;
+
+  RCLCPP_PUBLIC
+  const std::vector<int64_t> &
+  as_integer_array() const;
+
+  RCLCPP_PUBLIC
+  const std::vector<double> &
+  as_double_array() const;
+
+  RCLCPP_PUBLIC
+  const std::vector<std::string> &
+  as_string_array() const;
+
+  RCLCPP_PUBLIC
+  static Parameter
+  from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter);
 
   RCLCPP_PUBLIC
   rcl_interfaces::msg::Parameter
-  to_parameter();
+  to_parameter_msg() const;
 
   RCLCPP_PUBLIC
   std::string
@@ -212,24 +126,22 @@ public:
 
 private:
   std::string name_;
-  rcl_interfaces::msg::ParameterValue value_;
+  ParameterValue value_;
 };
 
-
 /// Return a json encoded version of the parameter intended for a dict.
 RCLCPP_PUBLIC
 std::string
-_to_json_dict_entry(const ParameterVariant & param);
+_to_json_dict_entry(const Parameter & param);
 
 RCLCPP_PUBLIC
 std::ostream &
-operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv);
+operator<<(std::ostream & os, const rclcpp::Parameter & pv);
 
 RCLCPP_PUBLIC
 std::ostream &
-operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters);
+operator<<(std::ostream & os, const std::vector<Parameter> & parameters);
 
-}  // namespace parameter
 }  // namespace rclcpp
 
 namespace std
@@ -238,12 +150,12 @@ namespace std
 /// Return a json encoded version of the parameter intended for a list.
 RCLCPP_PUBLIC
 std::string
-to_string(const rclcpp::parameter::ParameterVariant & param);
+to_string(const rclcpp::Parameter & param);
 
 /// Return a json encoded version of a vector of parameters, as a string.
 RCLCPP_PUBLIC
 std::string
-to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+to_string(const std::vector<rclcpp::Parameter> & parameters);
 
 }  // namespace std
 

rclcpp/include/rclcpp/parameter_client.hpp

@@ -68,25 +68,25 @@ public:
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
-  std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
+  std::shared_future<std::vector<rclcpp::Parameter>>
   get_parameters(
     const std::vector<std::string> & names,
     std::function<
-      void(std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)
+      void(std::shared_future<std::vector<rclcpp::Parameter>>)
     > callback = nullptr);
 
   RCLCPP_PUBLIC
-  std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
+  std::shared_future<std::vector<rclcpp::ParameterType>>
   get_parameter_types(
     const std::vector<std::string> & names,
     std::function<
-      void(std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)
+      void(std::shared_future<std::vector<rclcpp::ParameterType>>)
     > callback = nullptr);
 
   RCLCPP_PUBLIC
   std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
   set_parameters(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
+    const std::vector<rclcpp::Parameter> & parameters,
     std::function<
       void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
     > callback = nullptr);
@@ -94,7 +94,7 @@ public:
   RCLCPP_PUBLIC
   std::shared_future<rcl_interfaces::msg::SetParametersResult>
   set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
+    const std::vector<rclcpp::Parameter> & parameters,
     std::function<
       void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)
     > callback = nullptr);
@@ -120,8 +120,9 @@ public:
     auto msg_mem_strat =
       MessageMemoryStrategy<rcl_interfaces::msg::ParameterEvent, Alloc>::create_default();
 
+    using rcl_interfaces::msg::ParameterEvent;
     return rclcpp::create_subscription<
-      rcl_interfaces::msg::ParameterEvent, CallbackT, Alloc, SubscriptionT>(
+      ParameterEvent, CallbackT, Alloc, ParameterEvent, SubscriptionT>(
       this->node_topics_interface_.get(),
       "parameter_events",
       std::forward<CallbackT>(callback),
@@ -185,7 +186,7 @@ public:
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
   RCLCPP_PUBLIC
-  std::vector<rclcpp::parameter::ParameterVariant>
+  std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & parameter_names);
 
   RCLCPP_PUBLIC
@@ -200,7 +201,7 @@ public:
     std::vector<std::string> names;
     names.push_back(parameter_name);
     auto vars = get_parameters(names);
-    if ((vars.size() != 1) || (vars[0].get_type() == rclcpp::parameter::PARAMETER_NOT_SET)) {
+    if ((vars.size() != 1) || (vars[0].get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET)) {
       return parameter_not_found_handler();
     } else {
       return static_cast<T>(vars[0].get_value<T>());
@@ -226,16 +227,16 @@ public:
   }
 
   RCLCPP_PUBLIC
-  std::vector<rclcpp::parameter::ParameterType>
+  std::vector<rclcpp::ParameterType>
   get_parameter_types(const std::vector<std::string> & parameter_names);
 
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_PUBLIC
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_PUBLIC
   rcl_interfaces::msg::ListParametersResult

rclcpp/include/rclcpp/parameter_map.hpp

@@ -0,0 +1,53 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__PARAMETER_MAP_HPP_
+#define RCLCPP__PARAMETER_MAP_HPP_
+
+#include <rcl_yaml_param_parser/types.h>
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// A map of fully qualified node names to a list of parameters
+using ParameterMap = std::unordered_map<std::string, std::vector<Parameter>>;
+
+/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
+/// \param[in] c_params C structures containing parameters for multiple nodes.
+/// \returns a map where the keys are fully qualified node names and values a list of parameters.
+/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterMap
+parameter_map_from(const rcl_params_t * const c_params);
+
+/// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
+/// \param[in] c_value C structure containing a value of a parameter.
+/// \returns an instance of a parameter value
+/// \throws InvalidParameterValueException if the `rcl_variant_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterValue
+parameter_value_from(const rcl_variant_t * const c_value);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_MAP_HPP_

rclcpp/include/rclcpp/parameter_service.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__PARAMETER_SERVICE_HPP_
 #define RCLCPP__PARAMETER_SERVICE_HPP_
 
+#include <memory>
 #include <string>
 
 #include "rcl_interfaces/srv/describe_parameters.hpp"
@@ -40,11 +41,12 @@ public:
 
   RCLCPP_PUBLIC
   explicit ParameterService(
-    const rclcpp::Node::SharedPtr node,
+    const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+    const std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+    node_interfaces::NodeParametersInterface * node_params,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
 private:
-  const rclcpp::Node::SharedPtr node_;
   rclcpp::Service<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_service_;
   rclcpp::Service<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
     get_parameter_types_service_;

rclcpp/include/rclcpp/parameter_value.hpp

@@ -0,0 +1,316 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__PARAMETER_VALUE_HPP_
+#define RCLCPP__PARAMETER_VALUE_HPP_
+
+#include <exception>
+#include <iostream>
+#include <ostream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "rcl_interfaces/msg/parameter_type.hpp"
+#include "rcl_interfaces/msg/parameter_value.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+enum ParameterType
+{
+  PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
+  PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
+  PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
+  PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
+  PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
+  PARAMETER_BYTE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY,
+  PARAMETER_BOOL_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY,
+  PARAMETER_INTEGER_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
+  PARAMETER_DOUBLE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
+  PARAMETER_STRING_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY,
+};
+
+/// Return the name of a parameter type
+RCLCPP_PUBLIC
+std::string
+to_string(const ParameterType type);
+
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const ParameterType type);
+
+/// Indicate the parameter type does not match the expected type.
+class ParameterTypeException : public std::runtime_error
+{
+public:
+  /// Construct an instance.
+  /**
+   * \param[in] expected the expected parameter type.
+   * \param[in] actual the actual parameter type.
+   */
+  RCLCPP_PUBLIC
+  ParameterTypeException(ParameterType expected, ParameterType actual)
+  : std::runtime_error("expected [" + to_string(expected) + "] got [" + to_string(actual) + "]")
+  {}
+};
+
+/// Store the type and value of a parameter.
+class ParameterValue
+{
+public:
+  /// Construct a parameter value with type PARAMETER_NOT_SET.
+  RCLCPP_PUBLIC
+  ParameterValue();
+  /// Construct a parameter value from a message.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const rcl_interfaces::msg::ParameterValue & value);
+  /// Construct a parameter value with type PARAMETER_BOOL.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const bool bool_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const int int_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const int64_t int_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const float double_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const double double_value);
+  /// Construct a parameter value with type PARAMETER_STRING.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::string & string_value);
+  /// Construct a parameter value with type PARAMETER_STRING.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const char * string_value);
+  /// Construct a parameter value with type PARAMETER_BYTE_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<uint8_t> & byte_array_value);
+  /// Construct a parameter value with type PARAMETER_BOOL_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<bool> & bool_array_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<int> & int_array_value);
+  /// Construct a parameter value with type PARAMETER_INTEGER_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<int64_t> & int_array_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<float> & double_array_value);
+  /// Construct a parameter value with type PARAMETER_DOUBLE_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<double> & double_array_value);
+  /// Construct a parameter value with type PARAMETER_STRING_ARRAY.
+  RCLCPP_PUBLIC
+  explicit ParameterValue(const std::vector<std::string> & string_array_value);
+
+  /// Return an enum indicating the type of the set value.
+  RCLCPP_PUBLIC
+  ParameterType
+  get_type() const;
+
+  /// Return a message populated with the parameter value
+  RCLCPP_PUBLIC
+  rcl_interfaces::msg::ParameterValue
+  to_value_msg() const;
+
+  // The following get() variants require the use of ParameterType
+
+  template<ParameterType type>
+  typename std::enable_if<type == ParameterType::PARAMETER_BOOL, bool>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) {
+      throw ParameterTypeException(ParameterType::PARAMETER_BOOL, get_type());
+    }
+    return value_.bool_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<type == ParameterType::PARAMETER_INTEGER, int64_t>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) {
+      throw ParameterTypeException(ParameterType::PARAMETER_INTEGER, get_type());
+    }
+    return value_.integer_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<type == ParameterType::PARAMETER_DOUBLE, double>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
+      throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE, get_type());
+    }
+    return value_.double_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<type == ParameterType::PARAMETER_STRING, const std::string &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
+      throw ParameterTypeException(ParameterType::PARAMETER_STRING, get_type());
+    }
+    return value_.string_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector<uint8_t> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_BYTE_ARRAY, get_type());
+    }
+    return value_.byte_array_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_BOOL_ARRAY, const std::vector<bool> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_BOOL_ARRAY, get_type());
+    }
+    return value_.bool_array_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_INTEGER_ARRAY, const std::vector<int64_t> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_INTEGER_ARRAY, get_type());
+    }
+    return value_.integer_array_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_DOUBLE_ARRAY, const std::vector<double> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE_ARRAY, get_type());
+    }
+    return value_.double_array_value;
+  }
+
+  template<ParameterType type>
+  typename std::enable_if<
+    type == ParameterType::PARAMETER_STRING_ARRAY, const std::vector<std::string> &>::type
+  get() const
+  {
+    if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY) {
+      throw ParameterTypeException(ParameterType::PARAMETER_STRING_ARRAY, get_type());
+    }
+    return value_.string_array_value;
+  }
+
+  // The following get() variants allow the use of primitive types
+
+  template<typename type>
+  typename std::enable_if<std::is_same<type, bool>::value, bool>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_BOOL>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_integral<type>::value && !std::is_same<type, bool>::value, 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
+  get() const
+  {
+    return get<ParameterType::PARAMETER_DOUBLE>();
+  }
+
+  template<typename type>
+  typename std::enable_if<std::is_convertible<type, std::string>::value, const std::string &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_STRING>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<uint8_t> &>::value, const std::vector<uint8_t> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_BYTE_ARRAY>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<bool> &>::value, const std::vector<bool> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_BOOL_ARRAY>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<int64_t> &>::value, const std::vector<int64_t> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_INTEGER_ARRAY>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<double> &>::value, const std::vector<double> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_DOUBLE_ARRAY>();
+  }
+
+  template<typename type>
+  typename std::enable_if<
+    std::is_convertible<
+      type, const std::vector<std::string> &>::value, const std::vector<std::string> &>::type
+  get() const
+  {
+    return get<ParameterType::PARAMETER_STRING_ARRAY>();
+  }
+
+private:
+  rcl_interfaces::msg::ParameterValue value_;
+};
+
+/// Return the value of a parameter as a string
+RCLCPP_PUBLIC
+std::string
+to_string(const ParameterValue & type);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_VALUE_HPP_

rclcpp/include/rclcpp/publisher.hpp

@@ -205,11 +205,8 @@ public:
       ipm.publisher_id = intra_process_publisher_id_;
       ipm.message_sequence = message_seq;
       auto status = rcl_publish(&intra_process_publisher_handle_, &ipm);
-      if (status != RCL_RET_OK) {
-        // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-        throw std::runtime_error(
-          std::string("failed to publish intra process message: ") + rcl_get_error_string_safe());
-        // *INDENT-ON*
+      if (RCL_RET_OK != status) {
+        rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish intra process message");
       }
     } else {
       // Always destroy the message, even if we don't consume it, for consistency.
@@ -279,6 +276,19 @@ public:
     return this->publish(*msg);
   }
 
+  void
+  publish(const rcl_serialized_message_t * serialized_msg)
+  {
+    if (store_intra_process_message_) {
+      // TODO(Karsten1987): support serialized message passed by intraprocess
+      throw std::runtime_error("storing serialized messages in intra process is not supported yet");
+    }
+    auto status = rcl_publish_serialized_message(&publisher_handle_, serialized_msg);
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message");
+    }
+  }
+
   std::shared_ptr<MessageAlloc> get_allocator() const
   {
     return message_allocator_;
@@ -289,11 +299,8 @@ protected:
   do_inter_process_publish(const MessageT * msg)
   {
     auto status = rcl_publish(&publisher_handle_, msg);
-    if (status != RCL_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to publish message: ") + rcl_get_error_string_safe());
-      // *INDENT-ON*
+    if (RCL_RET_OK != status) {
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message");
     }
   }
 

rclcpp/include/rclcpp/publisher_factory.hpp

@@ -46,19 +46,19 @@ struct PublisherFactory
 {
   // Creates a PublisherT<MessageT, ...> publisher object and returns it as a PublisherBase.
   using PublisherFactoryFunction = std::function<
-      rclcpp::PublisherBase::SharedPtr(
-        rclcpp::node_interfaces::NodeBaseInterface * node_base,
-        const std::string & topic_name,
-        rcl_publisher_options_t & publisher_options)>;
+    rclcpp::PublisherBase::SharedPtr(
+      rclcpp::node_interfaces::NodeBaseInterface * node_base,
+      const std::string & topic_name,
+      rcl_publisher_options_t & publisher_options)>;
 
   PublisherFactoryFunction create_typed_publisher;
 
   // Adds the PublisherBase to the intraprocess manager with the correctly
   // templated call to IntraProcessManager::store_intra_process_message.
   using AddPublisherToIntraProcessManagerFunction = std::function<
-      uint64_t(
-        rclcpp::intra_process_manager::IntraProcessManager * ipm,
-        rclcpp::PublisherBase::SharedPtr publisher)>;
+    uint64_t(
+      rclcpp::intra_process_manager::IntraProcessManager * ipm,
+      rclcpp::PublisherBase::SharedPtr publisher)>;
 
   AddPublisherToIntraProcessManagerFunction add_publisher_to_intra_process_manager;
 
@@ -66,8 +66,8 @@ struct PublisherFactory
   // PublisherT::publish() and which handles the intra process transmission of
   // the message being published.
   using SharedPublishCallbackFactoryFunction = std::function<
-      rclcpp::PublisherBase::StoreMessageCallbackT(
-        rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm)>;
+    rclcpp::PublisherBase::StoreMessageCallbackT(
+      rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm)>;
 
   SharedPublishCallbackFactoryFunction create_shared_publish_callback;
 };

rclcpp/include/rclcpp/rclcpp.hpp

@@ -49,10 +49,12 @@
  *   - rclcpp::Node::describe_parameters()
  *   - rclcpp::Node::list_parameters()
  *   - rclcpp::Node::register_param_change_callback()
- *   - rclcpp::parameter::ParameterVariant
+ *   - rclcpp::Parameter
+ *   - rclcpp::ParameterValue
  *   - rclcpp::AsyncParametersClient
  *   - rclcpp::SyncParametersClient
  *   - rclcpp/parameter.hpp
+ *   - rclcpp/parameter_value.hpp
  *   - rclcpp/parameter_client.hpp
  *   - rclcpp/parameter_service.hpp
  * - Rate:

rclcpp/include/rclcpp/service.hpp

@@ -30,6 +30,7 @@
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/logging.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -41,11 +42,6 @@ class ServiceBase
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase)
 
-  RCLCPP_PUBLIC
-  ServiceBase(
-    std::shared_ptr<rcl_node_t> node_handle,
-    const std::string & service_name);
-
   RCLCPP_PUBLIC
   explicit ServiceBase(
     std::shared_ptr<rcl_node_t> node_handle);
@@ -54,15 +50,15 @@ public:
   virtual ~ServiceBase();
 
   RCLCPP_PUBLIC
-  std::string
+  const char *
   get_service_name();
 
   RCLCPP_PUBLIC
-  rcl_service_t *
+  std::shared_ptr<rcl_service_t>
   get_service_handle();
 
   RCLCPP_PUBLIC
-  const rcl_service_t *
+  std::shared_ptr<const rcl_service_t>
   get_service_handle() const;
 
   virtual std::shared_ptr<void> create_request() = 0;
@@ -84,8 +80,7 @@ protected:
 
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  rcl_service_t * service_handle_ = nullptr;
-  std::string service_name_;
+  std::shared_ptr<rcl_service_t> service_handle_;
   bool owns_rcl_handle_ = true;
 };
 
@@ -94,15 +89,15 @@ class Service : public ServiceBase
 {
 public:
   using CallbackType = std::function<
-      void(
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>)>;
+    void(
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>)>;
 
   using CallbackWithHeaderType = std::function<
-      void(
-        const std::shared_ptr<rmw_request_id_t>,
-        const std::shared_ptr<typename ServiceT::Request>,
-        std::shared_ptr<typename ServiceT::Response>)>;
+    void(
+      const std::shared_ptr<rmw_request_id_t>,
+      const std::shared_ptr<typename ServiceT::Request>,
+      std::shared_ptr<typename ServiceT::Response>)>;
   RCLCPP_SMART_PTR_DEFINITIONS(Service)
 
   Service(
@@ -110,17 +105,37 @@ public:
     const std::string & service_name,
     AnyServiceCallback<ServiceT> any_callback,
     rcl_service_options_t & service_options)
-  : ServiceBase(node_handle, service_name), any_callback_(any_callback)
+  : ServiceBase(node_handle), any_callback_(any_callback)
   {
     using rosidl_typesupport_cpp::get_service_type_support_handle;
     auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
 
+    std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
     // rcl does the static memory allocation here
-    service_handle_ = new rcl_service_t;
-    *service_handle_ = rcl_get_zero_initialized_service();
+    service_handle_ = std::shared_ptr<rcl_service_t>(
+      new rcl_service_t, [weak_node_handle](rcl_service_t * service)
+      {
+        auto handle = weak_node_handle.lock();
+        if (handle) {
+          if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
+            RCLCPP_ERROR(
+              rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
+              "Error in destruction of rcl service handle: %s",
+              rcl_get_error_string_safe());
+            rcl_reset_error();
+          }
+        } else {
+          RCLCPP_ERROR(
+            rclcpp::get_logger("rclcpp"),
+            "Error in destruction of rcl service handle: "
+            "the Node Handle was destructed too early. You will leak memory");
+        }
+        delete service;
+      });
+    *service_handle_.get() = rcl_get_zero_initialized_service();
 
     rcl_ret_t ret = rcl_service_init(
-      service_handle_,
+      service_handle_.get(),
       node_handle.get(),
       service_type_support_handle,
       service_name.c_str(),
@@ -141,6 +156,24 @@ public:
     }
   }
 
+  Service(
+    std::shared_ptr<rcl_node_t> node_handle,
+    std::shared_ptr<rcl_service_t> service_handle,
+    AnyServiceCallback<ServiceT> any_callback)
+  : ServiceBase(node_handle),
+    any_callback_(any_callback)
+  {
+    // check if service handle was initialized
+    if (!rcl_service_is_valid(service_handle.get(), nullptr)) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(
+        std::string("rcl_service_t in constructor argument must be initialized beforehand."));
+      // *INDENT-ON*
+    }
+
+    service_handle_ = service_handle;
+  }
+
   Service(
     std::shared_ptr<rcl_node_t> node_handle,
     rcl_service_t * service_handle,
@@ -149,35 +182,22 @@ public:
     any_callback_(any_callback)
   {
     // check if service handle was initialized
-    // TODO(karsten1987): Take this verification
-    // directly in rcl_*_t
-    // see: https://github.com/ros2/rcl/issues/81
-    if (!service_handle->impl) {
+    if (!rcl_service_is_valid(service_handle, nullptr)) {
       // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       throw std::runtime_error(
         std::string("rcl_service_t in constructor argument must be initialized beforehand."));
       // *INDENT-ON*
     }
-    service_handle_ = service_handle;
-    service_name_ = std::string(rcl_service_get_service_name(service_handle));
-    owns_rcl_handle_ = false;
+
+    // In this case, rcl owns the service handle memory
+    service_handle_ = std::shared_ptr<rcl_service_t>(new rcl_service_t);
+    service_handle_->impl = service_handle->impl;
   }
 
   Service() = delete;
 
   virtual ~Service()
   {
-    // check if you have ownership of the handle
-    if (owns_rcl_handle_) {
-      if (rcl_service_fini(service_handle_, node_handle_.get()) != RCL_RET_OK) {
-        std::stringstream ss;
-        ss << "Error in destruction of rcl service_handle_ handle: " <<
-          rcl_get_error_string_safe() << '\n';
-        (std::cerr << ss.str()).flush();
-        rcl_reset_error();
-      }
-      delete service_handle_;
-    }
   }
 
   std::shared_ptr<void> create_request()
@@ -206,7 +226,7 @@ public:
     std::shared_ptr<rmw_request_id_t> req_id,
     std::shared_ptr<typename ServiceT::Response> response)
   {
-    rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get());
+    rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get());
 
     if (status != RCL_RET_OK) {
       rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -25,6 +25,8 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcutils/logging_macros.h"
+
 #include "rmw/types.h"
 
 namespace rclcpp
@@ -46,21 +48,16 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>)
 
-  using ExecAllocTraits = allocator::AllocRebind<executor::AnyExecutable, Alloc>;
-  using ExecAlloc = typename ExecAllocTraits::allocator_type;
-  using ExecDeleter = allocator::Deleter<ExecAlloc, executor::AnyExecutable>;
   using VoidAllocTraits = typename allocator::AllocRebind<void *, Alloc>;
   using VoidAlloc = typename VoidAllocTraits::allocator_type;
 
   explicit AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
-    executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());
     allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
   }
 
   AllocatorMemoryStrategy()
   {
-    executable_allocator_ = std::make_shared<ExecAlloc>();
     allocator_ = std::make_shared<VoidAlloc>();
   }
 
@@ -96,22 +93,22 @@ public:
   {
     for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
       if (!wait_set->subscriptions[i]) {
-        subscription_handles_[i] = nullptr;
+        subscription_handles_[i].reset();
       }
     }
     for (size_t i = 0; i < wait_set->size_of_services; ++i) {
       if (!wait_set->services[i]) {
-        service_handles_[i] = nullptr;
+        service_handles_[i].reset();
       }
     }
     for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
       if (!wait_set->clients[i]) {
-        client_handles_[i] = nullptr;
+        client_handles_[i].reset();
       }
     }
     for (size_t i = 0; i < wait_set->size_of_timers; ++i) {
       if (!wait_set->timers[i]) {
-        timer_handles_[i] = nullptr;
+        timer_handles_[i].reset();
       }
     }
 
@@ -186,36 +183,46 @@ public:
   bool add_handles_to_wait_set(rcl_wait_set_t * wait_set)
   {
     for (auto subscription : subscription_handles_) {
-      if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add subscription to wait set: %s\n", rcl_get_error_string_safe());
+      if (rcl_wait_set_add_subscription(wait_set, subscription.get()) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add subscription to wait set: %s", rcl_get_error_string_safe());
         return false;
       }
     }
 
     for (auto client : client_handles_) {
-      if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add client to wait set: %s\n", rcl_get_error_string_safe());
+      if (rcl_wait_set_add_client(wait_set, client.get()) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add client to wait set: %s", rcl_get_error_string_safe());
         return false;
       }
     }
 
     for (auto service : service_handles_) {
-      if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add service to wait set: %s\n", rcl_get_error_string_safe());
+      if (rcl_wait_set_add_service(wait_set, service.get()) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add service to wait set: %s", rcl_get_error_string_safe());
         return false;
       }
     }
 
     for (auto timer : timer_handles_) {
-      if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add timer to wait set: %s\n", rcl_get_error_string_safe());
+      if (rcl_wait_set_add_timer(wait_set, timer.get()) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add timer to wait set: %s", rcl_get_error_string_safe());
         return false;
       }
     }
 
     for (auto guard_condition : guard_conditions_) {
       if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) {
-        fprintf(stderr, "Couldn't add guard_condition to wait set: %s\n",
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add guard_condition to wait set: %s",
           rcl_get_error_string_safe());
         return false;
       }
@@ -223,16 +230,9 @@ public:
     return true;
   }
 
-  /// Provide a newly initialized AnyExecutable object.
-  // \return Shared pointer to the fresh executable.
-  executor::AnyExecutable::SharedPtr instantiate_next_executable()
-  {
-    return std::allocate_shared<executor::AnyExecutable>(*executable_allocator_.get());
-  }
-
   virtual void
   get_next_subscription(
-    executor::AnyExecutable::SharedPtr any_exec,
+    executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes)
   {
     auto it = subscription_handles_.begin();
@@ -249,7 +249,7 @@ public:
         if (!group) {
           // Group was not found, meaning the subscription is not valid...
           // Remove it from the ready list and continue looking
-          subscription_handles_.erase(it);
+          it = subscription_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -260,23 +260,23 @@ public:
         }
         // Otherwise it is safe to set and return the any_exec
         if (is_intra_process) {
-          any_exec->subscription_intra_process = subscription;
+          any_exec.subscription_intra_process = subscription;
         } else {
-          any_exec->subscription = subscription;
+          any_exec.subscription = subscription;
         }
-        any_exec->callback_group = group;
-        any_exec->node_base = get_node_by_group(group, weak_nodes);
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
         subscription_handles_.erase(it);
         return;
       }
       // Else, the subscription is no longer valid, remove it and continue
-      subscription_handles_.erase(it);
+      it = subscription_handles_.erase(it);
     }
   }
 
   virtual void
   get_next_service(
-    executor::AnyExecutable::SharedPtr any_exec,
+    executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes)
   {
     auto it = service_handles_.begin();
@@ -288,7 +288,7 @@ public:
         if (!group) {
           // Group was not found, meaning the service is not valid...
           // Remove it from the ready list and continue looking
-          service_handles_.erase(it);
+          it = service_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -298,19 +298,19 @@ public:
           continue;
         }
         // Otherwise it is safe to set and return the any_exec
-        any_exec->service = service;
-        any_exec->callback_group = group;
-        any_exec->node_base = get_node_by_group(group, weak_nodes);
+        any_exec.service = service;
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
         service_handles_.erase(it);
         return;
       }
       // Else, the service is no longer valid, remove it and continue
-      service_handles_.erase(it);
+      it = service_handles_.erase(it);
     }
   }
 
   virtual void
-  get_next_client(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes)
+  get_next_client(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes)
   {
     auto it = client_handles_.begin();
     while (it != client_handles_.end()) {
@@ -321,7 +321,7 @@ public:
         if (!group) {
           // Group was not found, meaning the service is not valid...
           // Remove it from the ready list and continue looking
-          client_handles_.erase(it);
+          it = client_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -331,14 +331,14 @@ public:
           continue;
         }
         // Otherwise it is safe to set and return the any_exec
-        any_exec->client = client;
-        any_exec->callback_group = group;
-        any_exec->node_base = get_node_by_group(group, weak_nodes);
+        any_exec.client = client;
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
         client_handles_.erase(it);
         return;
       }
       // Else, the service is no longer valid, remove it and continue
-      client_handles_.erase(it);
+      it = client_handles_.erase(it);
     }
   }
 
@@ -375,16 +375,15 @@ public:
 private:
   template<typename T>
   using VectorRebind =
-      std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
+    std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
 
   VectorRebind<const rcl_guard_condition_t *> guard_conditions_;
 
-  VectorRebind<const rcl_subscription_t *> subscription_handles_;
-  VectorRebind<const rcl_service_t *> service_handles_;
-  VectorRebind<const rcl_client_t *> client_handles_;
-  VectorRebind<const rcl_timer_t *> timer_handles_;
+  VectorRebind<std::shared_ptr<const rcl_subscription_t>> subscription_handles_;
+  VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;
+  VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
+  VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
 
-  std::shared_ptr<ExecAlloc> executable_allocator_;
   std::shared_ptr<VoidAlloc> allocator_;
 };
 

rclcpp/include/rclcpp/subscription.hpp

@@ -29,12 +29,13 @@
 
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
+#include "rclcpp/any_subscription_callback.hpp"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
-#include "rclcpp/any_subscription_callback.hpp"
+#include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/type_support_decl.hpp"
-#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -64,7 +65,8 @@ public:
     std::shared_ptr<rcl_node_t> node_handle,
     const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
-    const rcl_subscription_options_t & subscription_options);
+    const rcl_subscription_options_t & subscription_options,
+    bool is_serialized = false);
 
   /// Default destructor.
   RCLCPP_PUBLIC
@@ -76,21 +78,27 @@ public:
   get_topic_name() const;
 
   RCLCPP_PUBLIC
-  rcl_subscription_t *
+  std::shared_ptr<rcl_subscription_t>
   get_subscription_handle();
 
   RCLCPP_PUBLIC
-  const rcl_subscription_t *
+  const std::shared_ptr<rcl_subscription_t>
   get_subscription_handle() const;
 
   RCLCPP_PUBLIC
-  virtual const rcl_subscription_t *
+  virtual const std::shared_ptr<rcl_subscription_t>
   get_intra_process_subscription_handle() const;
 
   /// Borrow a new message.
   /** \return Shared pointer to the fresh message. */
   virtual std::shared_ptr<void>
   create_message() = 0;
+
+  /// Borrow a new serialized message
+  /** \return Shared pointer to a rcl_message_serialized_t. */
+  virtual std::shared_ptr<rcl_serialized_message_t>
+  create_serialized_message() = 0;
+
   /// Check if we need to handle the message, and execute the callback if we do.
   /**
    * \param[in] message Shared pointer to the message to handle.
@@ -104,31 +112,47 @@ public:
   virtual void
   return_message(std::shared_ptr<void> & message) = 0;
 
+  /// Return the message borrowed in create_serialized_message.
+  /** \param[in] message Shared pointer to the returned message. */
+  virtual void
+  return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message) = 0;
+
   virtual void
   handle_intra_process_message(
     rcl_interfaces::msg::IntraProcessMessage & ipm,
     const rmw_message_info_t & message_info) = 0;
 
+  const rosidl_message_type_support_t &
+  get_message_type_support_handle() const;
+
+  bool
+  is_serialized() const;
+
 protected:
-  rcl_subscription_t intra_process_subscription_handle_ = rcl_get_zero_initialized_subscription();
-  rcl_subscription_t subscription_handle_ = rcl_get_zero_initialized_subscription();
+  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_;
 
 private:
   RCLCPP_DISABLE_COPY(SubscriptionBase)
+
+  rosidl_message_type_support_t type_support_;
+  bool is_serialized_;
 };
 
 /// Subscription implementation, templated on the type of message this subscription receives.
-template<typename MessageT, typename Alloc = std::allocator<void>>
+template<
+  typename CallbackMessageT,
+  typename Alloc = std::allocator<void>>
 class Subscription : public SubscriptionBase
 {
   friend class rclcpp::node_interfaces::NodeTopicsInterface;
 
 public:
-  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageAllocTraits = allocator::AllocRebind<CallbackMessageT, Alloc>;
   using MessageAlloc = typename MessageAllocTraits::allocator_type;
-  using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
-  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+  using MessageDeleter = allocator::Deleter<MessageAlloc, CallbackMessageT>;
+  using MessageUniquePtr = std::unique_ptr<CallbackMessageT, MessageDeleter>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Subscription)
 
@@ -144,17 +168,19 @@ public:
    */
   Subscription(
     std::shared_ptr<rcl_node_t> node_handle,
+    const rosidl_message_type_support_t & ts,
     const std::string & topic_name,
     const rcl_subscription_options_t & subscription_options,
-    AnySubscriptionCallback<MessageT, Alloc> callback,
-    typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
-    memory_strategy = message_memory_strategy::MessageMemoryStrategy<MessageT,
+    AnySubscriptionCallback<CallbackMessageT, Alloc> callback,
+    typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, Alloc>::SharedPtr
+    memory_strategy = message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
     Alloc>::create_default())
   : SubscriptionBase(
       node_handle,
-      *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
+      ts,
       topic_name,
-      subscription_options),
+      subscription_options,
+      rclcpp::subscription_traits::is_serialized_subscription_argument<CallbackMessageT>::value),
     any_callback_(callback),
     message_memory_strategy_(memory_strategy),
     get_intra_process_message_callback_(nullptr),
@@ -167,11 +193,12 @@ public:
    * \param[in] message_memory_strategy Shared pointer to the memory strategy to set.
    */
   void set_message_memory_strategy(
-    typename message_memory_strategy::MessageMemoryStrategy<MessageT,
+    typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
     Alloc>::SharedPtr message_memory_strategy)
   {
     message_memory_strategy_ = message_memory_strategy;
   }
+
   std::shared_ptr<void> create_message()
   {
     /* The default message memory strategy provides a dynamically allocated message on each call to
@@ -181,6 +208,11 @@ public:
     return message_memory_strategy_->borrow_message();
   }
 
+  std::shared_ptr<rcl_serialized_message_t> create_serialized_message()
+  {
+    return message_memory_strategy_->borrow_serialized_message();
+  }
+
   void handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info)
   {
     if (matches_any_intra_process_publishers_) {
@@ -190,7 +222,7 @@ public:
         return;
       }
     }
-    auto typed_message = std::static_pointer_cast<MessageT>(message);
+    auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     any_callback_.dispatch(typed_message, message_info);
   }
 
@@ -198,10 +230,15 @@ public:
   /** \param message message to be returned */
   void return_message(std::shared_ptr<void> & message)
   {
-    auto typed_message = std::static_pointer_cast<MessageT>(message);
+    auto typed_message = std::static_pointer_cast<CallbackMessageT>(message);
     message_memory_strategy_->return_message(typed_message);
   }
 
+  void return_serialized_message(std::shared_ptr<rcl_serialized_message_t> & message)
+  {
+    message_memory_strategy_->return_serialized_message(message);
+  }
+
   void handle_intra_process_message(
     rcl_interfaces::msg::IntraProcessMessage & ipm,
     const rmw_message_info_t & message_info)
@@ -229,7 +266,7 @@ public:
   }
 
   using GetMessageCallbackType =
-      std::function<void(uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
+    std::function<void(uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
   using MatchesAnyPublishersCallbackType = std::function<bool(const rmw_gid_t *)>;
 
   /// Implemenation detail.
@@ -241,7 +278,7 @@ public:
   {
     std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
     rcl_ret_t ret = rcl_subscription_init(
-      &intra_process_subscription_handle_,
+      intra_process_subscription_handle_.get(),
       node_handle_.get(),
       rclcpp::type_support::get_intra_process_message_msg_type_support(),
       intra_process_topic_name.c_str(),
@@ -266,20 +303,20 @@ public:
   }
 
   /// Implemenation detail.
-  const rcl_subscription_t *
+  const std::shared_ptr<rcl_subscription_t>
   get_intra_process_subscription_handle() const
   {
     if (!get_intra_process_message_callback_) {
       return nullptr;
     }
-    return &intra_process_subscription_handle_;
+    return intra_process_subscription_handle_;
   }
 
 private:
   RCLCPP_DISABLE_COPY(Subscription)
 
-  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
-  typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  AnySubscriptionCallback<CallbackMessageT, Alloc> any_callback_;
+  typename message_memory_strategy::MessageMemoryStrategy<CallbackMessageT, Alloc>::SharedPtr
     message_memory_strategy_;
 
   GetMessageCallbackType get_intra_process_message_callback_;

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -25,6 +25,7 @@
 #include "rosidl_typesupport_cpp/message_type_support.hpp"
 
 #include "rclcpp/subscription.hpp"
+#include "rclcpp/subscription_traits.hpp"
 #include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -47,40 +48,46 @@ struct SubscriptionFactory
 {
   // Creates a Subscription<MessageT> object and returns it as a SubscriptionBase.
   using SubscriptionFactoryFunction = std::function<
-      rclcpp::SubscriptionBase::SharedPtr(
-        rclcpp::node_interfaces::NodeBaseInterface * node_base,
-        const std::string & topic_name,
-        rcl_subscription_options_t & subscription_options)>;
+    rclcpp::SubscriptionBase::SharedPtr(
+      rclcpp::node_interfaces::NodeBaseInterface * node_base,
+      const std::string & topic_name,
+      rcl_subscription_options_t & subscription_options)>;
 
   SubscriptionFactoryFunction create_typed_subscription;
 
   // Function that takes a MessageT from the intra process manager
   using SetupIntraProcessFunction = std::function<
-      void(
-        rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
-        rclcpp::SubscriptionBase::SharedPtr subscription,
-        const rcl_subscription_options_t & subscription_options)>;
+    void(
+      rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
+      rclcpp::SubscriptionBase::SharedPtr subscription,
+      const rcl_subscription_options_t & subscription_options)>;
 
   SetupIntraProcessFunction setup_intra_process;
 };
 
 /// Return a SubscriptionFactory with functions for creating a SubscriptionT<MessageT, Alloc>.
-template<typename MessageT, typename CallbackT, typename Alloc, typename SubscriptionT>
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename Alloc,
+  typename CallbackMessageT,
+  typename SubscriptionT>
 SubscriptionFactory
 create_subscription_factory(
   CallbackT && callback,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+    CallbackMessageT, Alloc>::SharedPtr
   msg_mem_strat,
   std::shared_ptr<Alloc> allocator)
 {
   SubscriptionFactory factory;
 
   using rclcpp::AnySubscriptionCallback;
-  AnySubscriptionCallback<MessageT, Alloc> any_subscription_callback(allocator);
+  AnySubscriptionCallback<CallbackMessageT, Alloc> any_subscription_callback(allocator);
   any_subscription_callback.set(std::forward<CallbackT>(callback));
 
   auto message_alloc =
-    std::make_shared<typename Subscription<MessageT, Alloc>::MessageAlloc>();
+    std::make_shared<typename Subscription<CallbackMessageT, Alloc>::MessageAlloc>();
 
   // factory function that creates a MessageT specific SubscriptionT
   factory.create_typed_subscription =
@@ -91,13 +98,14 @@ create_subscription_factory(
     ) -> rclcpp::SubscriptionBase::SharedPtr
     {
       subscription_options.allocator =
-        rclcpp::allocator::get_rcl_allocator<MessageT>(*message_alloc.get());
+        rclcpp::allocator::get_rcl_allocator<CallbackMessageT>(*message_alloc.get());
 
       using rclcpp::Subscription;
       using rclcpp::SubscriptionBase;
 
-      auto sub = Subscription<MessageT, Alloc>::make_shared(
+      auto sub = Subscription<CallbackMessageT, Alloc>::make_shared(
         node_base->get_shared_rcl_node_handle(),
+        *rosidl_typesupport_cpp::get_message_type_support_handle<MessageT>(),
         topic_name,
         subscription_options,
         any_subscription_callback,
@@ -117,7 +125,7 @@ create_subscription_factory(
       uint64_t intra_process_subscription_id = ipm->add_subscription(subscription);
 
       auto intra_process_options = rcl_subscription_get_default_options();
-      intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(
+      intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator<CallbackMessageT>(
         *message_alloc.get());
       intra_process_options.qos = subscription_options.qos;
       intra_process_options.ignore_local_publications = false;
@@ -128,7 +136,7 @@ create_subscription_factory(
         uint64_t publisher_id,
         uint64_t message_sequence,
         uint64_t subscription_id,
-        typename rclcpp::Subscription<MessageT, Alloc>::MessageUniquePtr & message)
+        typename rclcpp::Subscription<CallbackMessageT, Alloc>::MessageUniquePtr & message)
         {
           auto ipm = weak_ipm.lock();
           if (!ipm) {
@@ -136,7 +144,7 @@ create_subscription_factory(
             throw std::runtime_error(
                     "intra process take called after destruction of intra process manager");
           }
-          ipm->take_intra_process_message<MessageT, Alloc>(
+          ipm->take_intra_process_message<CallbackMessageT, Alloc>(
             publisher_id, message_sequence, subscription_id, message);
         };
 

rclcpp/include/rclcpp/subscription_traits.hpp

@@ -0,0 +1,80 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SUBSCRIPTION_TRAITS_HPP_
+#define RCLCPP__SUBSCRIPTION_TRAITS_HPP_
+
+#include <memory>
+
+#include "rclcpp/function_traits.hpp"
+
+namespace rclcpp
+{
+namespace subscription_traits
+{
+
+/*
+ * The current version of uncrustify has a misinterpretion here
+ * between `:` used for inheritance vs for initializer list
+ * The result is that whenever a templated struct is used,
+ * the colon has to be without any whitespace next to it whereas
+ * when no template is used, the colon has to be separated by a space.
+ * Cheers!
+ */
+template<typename T>
+struct is_serialized_subscription_argument : std::false_type
+{};
+
+template<>
+struct is_serialized_subscription_argument<rcl_serialized_message_t>: std::true_type
+{};
+
+template<>
+struct is_serialized_subscription_argument<std::shared_ptr<rcl_serialized_message_t>>
+  : std::true_type
+{};
+
+template<typename T>
+struct is_serialized_subscription : is_serialized_subscription_argument<T>
+{};
+
+template<typename CallbackT>
+struct is_serialized_callback
+  : is_serialized_subscription_argument<
+    typename rclcpp::function_traits::function_traits<CallbackT>::template argument_type<0>>
+{};
+
+template<typename MessageT>
+struct extract_message_type
+{
+  using type = typename std::remove_cv<MessageT>::type;
+};
+
+template<typename MessageT>
+struct extract_message_type<std::shared_ptr<MessageT>>: extract_message_type<MessageT>
+{};
+
+template<typename MessageT, typename Deleter>
+struct extract_message_type<std::unique_ptr<MessageT, Deleter>>: extract_message_type<MessageT>
+{};
+
+template<typename CallbackT>
+struct has_message_type : extract_message_type<
+    typename rclcpp::function_traits::function_traits<CallbackT>::template argument_type<0>>
+{};
+
+}  // namespace subscription_traits
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SUBSCRIPTION_TRAITS_HPP_

rclcpp/include/rclcpp/time.hpp

@@ -35,7 +35,7 @@ public:
   Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME);
 
   RCLCPP_PUBLIC
-  explicit Time(uint64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);
+  explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME);
 
   RCLCPP_PUBLIC
   Time(const Time & rhs);

rclcpp/include/rclcpp/time_source.hpp

@@ -21,6 +21,7 @@
 #include "rcl/time.h"
 
 #include "builtin_interfaces/msg/time.hpp"
+#include "rosgraph_msgs/msg/clock.hpp"
 #include "rcl_interfaces/msg/parameter_event.hpp"
 
 #include "rclcpp/node.hpp"
@@ -75,13 +76,13 @@ private:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
 
   // The subscription for the clock callback
-  using MessageT = builtin_interfaces::msg::Time;
+  using MessageT = rosgraph_msgs::msg::Clock;
   using Alloc = std::allocator<void>;
   using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
   std::shared_ptr<SubscriptionT> clock_subscription_;
 
   // The clock callback itself
-  void clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg);
+  void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg);
 
   // Parameter Client pointer
   std::shared_ptr<rclcpp::AsyncParametersClient> parameter_client_;
@@ -115,7 +116,7 @@ private:
   // This is needed when new clocks are added.
   bool ros_time_active_;
   // Last set message to be passed to newly registered clocks
-  builtin_interfaces::msg::Time::SharedPtr last_msg_set_;
+  rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_;
 
   // A lock to protect iterating the associated_clocks_ field.
   std::mutex clock_list_lock_;

rclcpp/include/rclcpp/timer.hpp

@@ -62,7 +62,7 @@ public:
   execute_callback() = 0;
 
   RCLCPP_PUBLIC
-  const rcl_timer_t *
+  std::shared_ptr<const rcl_timer_t>
   get_timer_handle();
 
   /// Check how long the timer has until its next scheduled callback.
@@ -85,7 +85,7 @@ public:
   bool is_ready();
 
 protected:
-  rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer();
+  std::shared_ptr<rcl_timer_t> timer_handle_;
 };
 
 
@@ -122,15 +122,12 @@ public:
   {
     // Stop the timer from running.
     cancel();
-    if (rcl_timer_fini(&timer_handle_) != RCL_RET_OK) {
-      fprintf(stderr, "Failed to clean up rcl timer handle: %s\n", rcl_get_error_string_safe());
-    }
   }
 
   void
   execute_callback()
   {
-    rcl_ret_t ret = rcl_timer_call(&timer_handle_);
+    rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
     if (ret == RCL_RET_TIMER_CANCELED) {
       return;
     }

rclcpp/include/rclcpp/utilities.hpp

@@ -17,6 +17,8 @@
 
 #include <chrono>
 #include <functional>
+#include <limits>
+#include <vector>
 
 #include "rclcpp/visibility_control.hpp"
 
@@ -44,7 +46,6 @@ std::string to_string(T value)
 
 namespace rclcpp
 {
-
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
  * \param[in] argc Number of arguments.
@@ -52,7 +53,33 @@ namespace rclcpp
  */
 RCLCPP_PUBLIC
 void
-init(int argc, char * argv[]);
+init(int argc, char const * const argv[]);
+
+/// Initialize communications via the rmw implementation and set up a global signal handler.
+/**
+ * Additionally removes ROS-specific arguments from the argument vector.
+ * \param[in] argc Number of arguments.
+ * \param[in] argv Argument vector.
+ * \returns Members of the argument vector that are not ROS arguments.
+ */
+RCLCPP_PUBLIC
+std::vector<std::string>
+init_and_remove_ros_arguments(int argc, char const * const argv[]);
+
+/// Remove ROS-specific arguments from argument vector.
+/**
+ * Some arguments may not have been intended as ROS arguments.
+ * This function populates a the aruments in a vector.
+ * Since the first argument is always assumed to be a process name, the vector
+ * will always contain the process name.
+ *
+ * \param[in] argc Number of arguments.
+ * \param[in] argv Argument vector.
+ * \returns Members of the argument vector that are not ROS arguments.
+ */
+RCLCPP_PUBLIC
+std::vector<std::string>
+remove_ros_arguments(int argc, char const * const argv[]);
 
 /// Check rclcpp's status.
 /** \return True if SIGINT hasn't fired yet, false otherwise. */
@@ -109,6 +136,74 @@ RCLCPP_PUBLIC
 bool
 sleep_for(const std::chrono::nanoseconds & nanoseconds);
 
+/// Safely check if addition will overflow.
+/**
+ * The type of the operands, T, should have defined
+ * std::numeric_limits<T>::max(), `>`, `<` and `-` operators.
+ *
+ * \param[in] x is the first addend.
+ * \param[in] y is the second addend.
+ * \tparam T is type of the operands.
+ * \return True if the x + y sum is greater than T::max value.
+ */
+template<typename T>
+bool
+add_will_overflow(const T x, const T y)
+{
+  return (y > 0) && (x > (std::numeric_limits<T>::max() - y));
+}
+
+/// Safely check if addition will underflow.
+/**
+ * The type of the operands, T, should have defined
+ * std::numeric_limits<T>::min(), `>`, `<` and `-` operators.
+ *
+ * \param[in] x is the first addend.
+ * \param[in] y is the second addend.
+ * \tparam T is type of the operands.
+ * \return True if the x + y sum is less than T::min value.
+ */
+template<typename T>
+bool
+add_will_underflow(const T x, const T y)
+{
+  return (y < 0) && (x < (std::numeric_limits<T>::min() - y));
+}
+
+/// Safely check if subtraction will overflow.
+/**
+ * The type of the operands, T, should have defined
+ * std::numeric_limits<T>::max(), `>`, `<` and `+` operators.
+ *
+ * \param[in] x is the minuend.
+ * \param[in] y is the subtrahend.
+ * \tparam T is type of the operands.
+ * \return True if the difference `x - y` sum is grater than T::max value.
+ */
+template<typename T>
+bool
+sub_will_overflow(const T x, const T y)
+{
+  return (y < 0) && (x > (std::numeric_limits<T>::max() + y));
+}
+
+/// Safely check if subtraction will underflow.
+/**
+ * The type of the operands, T, should have defined
+ * std::numeric_limits<T>::min(), `>`, `<` and `+` operators.
+ *
+ * \param[in] x is the minuend.
+ * \param[in] y is the subtrahend.
+ * \tparam T is type of the operands.
+ * \return True if the difference `x - y` sum is less than T::min value.
+ */
+template<typename T>
+bool
+sub_will_underflow(const T x, const T y)
+{
+  return (y > 0) && (x < (std::numeric_limits<T>::min() + y));
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__UTILITIES_HPP_

rclcpp/package.xml

@@ -2,27 +2,30 @@
 <?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.4.0</version>
+  <version>0.5.0</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
   <build_export_depend>rmw</build_export_depend>
 
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>rcl_interfaces</build_depend>
+  <build_depend>rosgraph_msgs</build_depend>
   <build_depend>rosidl_generator_cpp</build_depend>
   <build_depend>rosidl_typesupport_c</build_depend>
   <build_depend>rosidl_typesupport_cpp</build_depend>
   <build_export_depend>builtin_interfaces</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
+  <build_export_depend>rosgraph_msgs</build_export_depend>
   <build_export_depend>rosidl_generator_cpp</build_export_depend>
   <build_export_depend>rosidl_typesupport_c</build_export_depend>
   <build_export_depend>rosidl_typesupport_cpp</build_export_depend>
 
   <depend>rcl</depend>
+  <depend>rcl_yaml_param_parser</depend>
   <depend>rmw_implementation</depend>
 
   <exec_depend>ament_cmake</exec_depend>
@@ -33,6 +36,7 @@
   <test_depend>ament_lint_common</test_depend>
   <test_depend>rmw</test_depend>
   <test_depend>rmw_implementation_cmake</test_depend>
+  <test_depend>test_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rclcpp/src/rclcpp/client.cpp

@@ -14,6 +14,7 @@
 
 #include "rclcpp/client.hpp"
 
+#include <algorithm>
 #include <chrono>
 #include <cstdio>
 #include <memory>
@@ -26,6 +27,7 @@
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/logging.hpp"
 
 using rclcpp::ClientBase;
 using rclcpp::exceptions::InvalidNodeError;
@@ -33,39 +35,65 @@ using rclcpp::exceptions::throw_from_rcl_error;
 
 ClientBase::ClientBase(
   rclcpp::node_interfaces::NodeBaseInterface * node_base,
-  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
-  const std::string & service_name)
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph)
 : node_graph_(node_graph),
-  node_handle_(node_base->get_shared_rcl_node_handle()),
-  service_name_(service_name)
-{}
+  node_handle_(node_base->get_shared_rcl_node_handle())
+{
+  std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
+  client_handle_ = std::shared_ptr<rcl_client_t>(
+    new rcl_client_t, [weak_node_handle](rcl_client_t * client)
+    {
+      auto handle = weak_node_handle.lock();
+      if (handle) {
+        if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) {
+          RCLCPP_ERROR(
+            rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
+            "Error in destruction of rcl client handle: %s", rcl_get_error_string_safe());
+          rcl_reset_error();
+        }
+      } else {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"),
+          "Error in destruction of rcl client handle: "
+          "the Node Handle was destructed too early. You will leak memory");
+      }
+      delete client;
+    });
+  *client_handle_.get() = rcl_get_zero_initialized_client();
+}
 
-ClientBase::~ClientBase() {}
+ClientBase::~ClientBase()
+{
+  // Make sure the client handle is destructed as early as possible and before the node handle
+  client_handle_.reset();
+}
 
-const std::string &
+const char *
 ClientBase::get_service_name() const
 {
-  return this->service_name_;
+  return rcl_client_get_service_name(this->get_client_handle().get());
 }
 
-rcl_client_t *
+std::shared_ptr<rcl_client_t>
 ClientBase::get_client_handle()
 {
-  return &client_handle_;
+  return client_handle_;
 }
 
-const rcl_client_t *
+std::shared_ptr<const rcl_client_t>
 ClientBase::get_client_handle() const
 {
-  return &client_handle_;
+  return client_handle_;
 }
 
 bool
 ClientBase::service_is_ready() const
 {
   bool is_ready;
-  rcl_ret_t ret =
-    rcl_service_server_is_available(this->get_rcl_node_handle(), &client_handle_, &is_ready);
+  rcl_ret_t ret = rcl_service_server_is_available(
+    this->get_rcl_node_handle(),
+    this->get_client_handle().get(),
+    &is_ready);
   if (ret != RCL_RET_OK) {
     throw_from_rcl_error(ret, "rcl_service_server_is_available failed");
   }
@@ -103,12 +131,18 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
     if (!rclcpp::ok()) {
       return false;
     }
-    node_ptr->wait_for_graph_change(event, time_to_wait);
-    event->check_and_clear();  // reset the event
-
-    // always check if the service is ready, even if the graph event wasn't triggered
-    // this is needed to avoid a race condition that is specific to the Connext RMW implementation
+    // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation.
+    // A race condition means that graph changes for services becoming available may trigger the
+    // wait set to wake up, but then not be reported as ready immediately after the wake up
     // (see https://github.com/ros2/rmw_connext/issues/201)
+    // If no other graph events occur, the wait set will not be triggered again until the timeout
+    // has been reached, despite the service being available, so we artificially limit the wait
+    // time to limit the delay.
+    node_ptr->wait_for_graph_change(
+      event, std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100))));
+    // Because of the aforementioned race condition, we check if the service is ready even if the
+    // graph event wasn't triggered.
+    event->check_and_clear();
     if (this->service_is_ready()) {
       return true;
     }

rclcpp/src/rclcpp/duration.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <cmath>
 #include <cstdlib>
 #include <limits>
 #include <utility>
@@ -182,6 +183,34 @@ Duration::operator-(const rclcpp::Duration & rhs) const
     rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds);
 }
 
+void
+bounds_check_duration_scale(int64_t dns, double scale, uint64_t max)
+{
+  auto abs_dns = static_cast<uint64_t>(std::abs(dns));
+  auto abs_scale = std::abs(scale);
+
+  if (abs_scale > 1.0 && abs_dns > static_cast<uint64_t>(max / abs_scale)) {
+    if ((dns > 0 && scale > 0) || (dns < 0 && scale < 0)) {
+      throw std::overflow_error("duration scaling leads to int64_t overflow");
+    } else {
+      throw std::underflow_error("duration scaling leads to int64_t underflow");
+    }
+  }
+}
+
+Duration
+Duration::operator*(double scale) const
+{
+  if (!std::isfinite(scale)) {
+    throw std::runtime_error("abnormal scale in rclcpp::Duration");
+  }
+  bounds_check_duration_scale(
+    this->rcl_duration_.nanoseconds,
+    scale,
+    std::numeric_limits<rcl_duration_value_t>::max());
+  return Duration(static_cast<rcl_duration_value_t>(rcl_duration_.nanoseconds * scale));
+}
+
 rcl_duration_value_t
 Duration::nanoseconds() const
 {

rclcpp/src/rclcpp/executor.cpp

@@ -28,6 +28,8 @@
 
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
+#include "rcutils/logging_macros.h"
+
 using rclcpp::executor::AnyExecutable;
 using rclcpp::executor::Executor;
 using rclcpp::executor::ExecutorArgs;
@@ -59,12 +61,14 @@ Executor::Executor(const ExecutorArgs & args)
   if (rcl_wait_set_init(
       &wait_set_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK)
   {
-    fprintf(stderr,
-      "[rclcpp::error] failed to create wait set: %s\n", rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to create wait set: %s", rcl_get_error_string_safe());
     rcl_reset_error();
     if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
-      fprintf(stderr,
-        "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to destroy guard condition: %s", rcl_get_error_string_safe());
       rcl_reset_error();
     }
     throw std::runtime_error("Failed to create wait set in Executor constructor");
@@ -85,14 +89,16 @@ Executor::~Executor()
 
   // Finalize the wait set.
   if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
-    fprintf(stderr,
-      "[rclcpp::error] failed to destroy wait set: %s\n", rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to destroy wait set: %s", rcl_get_error_string_safe());
     rcl_reset_error();
   }
   // Finalize the interrupt guard condition.
   if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
-    fprintf(stderr,
-      "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "failed to destroy guard condition: %s", rcl_get_error_string_safe());
     rcl_reset_error();
   }
   // Remove and release the sigint guard condition
@@ -200,9 +206,13 @@ Executor::spin_some()
     throw std::runtime_error("spin_some() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  AnyExecutable::SharedPtr any_exec;
-  while ((any_exec = get_next_executable(std::chrono::milliseconds::zero())) && spinning.load()) {
-    execute_any_executable(any_exec);
+  while (spinning.load()) {
+    AnyExecutable any_exec;
+    if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) {
+      execute_any_executable(any_exec);
+    } else {
+      break;
+    }
   }
 }
 
@@ -213,8 +223,8 @@ Executor::spin_once(std::chrono::nanoseconds timeout)
     throw std::runtime_error("spin_once() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  auto any_exec = get_next_executable(timeout);
-  if (any_exec) {
+  AnyExecutable any_exec;
+  if (get_next_executable(any_exec, timeout)) {
     execute_any_executable(any_exec);
   }
 }
@@ -238,28 +248,28 @@ Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr
 }
 
 void
-Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec)
+Executor::execute_any_executable(AnyExecutable & any_exec)
 {
-  if (!any_exec || !spinning.load()) {
+  if (!spinning.load()) {
     return;
   }
-  if (any_exec->timer) {
-    execute_timer(any_exec->timer);
+  if (any_exec.timer) {
+    execute_timer(any_exec.timer);
   }
-  if (any_exec->subscription) {
-    execute_subscription(any_exec->subscription);
+  if (any_exec.subscription) {
+    execute_subscription(any_exec.subscription);
   }
-  if (any_exec->subscription_intra_process) {
-    execute_intra_process_subscription(any_exec->subscription_intra_process);
+  if (any_exec.subscription_intra_process) {
+    execute_intra_process_subscription(any_exec.subscription_intra_process);
   }
-  if (any_exec->service) {
-    execute_service(any_exec->service);
+  if (any_exec.service) {
+    execute_service(any_exec.service);
   }
-  if (any_exec->client) {
-    execute_client(any_exec->client);
+  if (any_exec.client) {
+    execute_client(any_exec.client);
   }
   // Reset the callback_group, regardless of type
-  any_exec->callback_group->can_be_taken_from().store(true);
+  any_exec.callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
   if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
@@ -271,21 +281,41 @@ void
 Executor::execute_subscription(
   rclcpp::SubscriptionBase::SharedPtr subscription)
 {
-  std::shared_ptr<void> message = subscription->create_message();
   rmw_message_info_t message_info;
+  message_info.from_intra_process = false;
 
-  auto ret = rcl_take(subscription->get_subscription_handle(),
+  if (subscription->is_serialized()) {
+    auto serialized_msg = subscription->create_serialized_message();
+    auto ret = rcl_take_serialized_message(
+      subscription->get_subscription_handle().get(),
+      serialized_msg.get(), &message_info);
+    if (RCL_RET_OK == ret) {
+      auto void_serialized_msg = std::static_pointer_cast<void>(serialized_msg);
+      subscription->handle_message(void_serialized_msg, message_info);
+    } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "take_serialized failed for subscription on topic '%s': %s",
+        subscription->get_topic_name(), rcl_get_error_string_safe());
+      rcl_reset_error();
+    }
+    subscription->return_serialized_message(serialized_msg);
+  } else {
+    std::shared_ptr<void> message = subscription->create_message();
+    auto ret = rcl_take(
+      subscription->get_subscription_handle().get(),
       message.get(), &message_info);
-  if (ret == RCL_RET_OK) {
-    message_info.from_intra_process = false;
-    subscription->handle_message(message, message_info);
-  } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
-    fprintf(stderr,
-      "[rclcpp::error] take failed for subscription on topic '%s': %s\n",
-      subscription->get_topic_name(), rcl_get_error_string_safe());
-    rcl_reset_error();
+    if (RCL_RET_OK == ret) {
+      subscription->handle_message(message, message_info);
+    } else if (RCL_RET_SUBSCRIPTION_TAKE_FAILED != ret) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "could not deserialize serialized message on topic '%s': %s",
+        subscription->get_topic_name(), rcl_get_error_string_safe());
+      rcl_reset_error();
+    }
+    subscription->return_message(message);
   }
-  subscription->return_message(message);
 }
 
 void
@@ -295,7 +325,7 @@ Executor::execute_intra_process_subscription(
   rcl_interfaces::msg::IntraProcessMessage ipm;
   rmw_message_info_t message_info;
   rcl_ret_t status = rcl_take(
-    subscription->get_intra_process_subscription_handle(),
+    subscription->get_intra_process_subscription_handle().get(),
     &ipm,
     &message_info);
 
@@ -303,8 +333,9 @@ Executor::execute_intra_process_subscription(
     message_info.from_intra_process = true;
     subscription->handle_intra_process_message(ipm, message_info);
   } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
-    fprintf(stderr,
-      "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "take failed for intra process subscription on topic '%s': %s",
       subscription->get_topic_name(), rcl_get_error_string_safe());
     rcl_reset_error();
   }
@@ -324,15 +355,16 @@ Executor::execute_service(
   auto request_header = service->create_request_header();
   std::shared_ptr<void> request = service->create_request();
   rcl_ret_t status = rcl_take_request(
-    service->get_service_handle(),
+    service->get_service_handle().get(),
     request_header.get(),
     request.get());
   if (status == RCL_RET_OK) {
     service->handle_request(request_header, request);
   } else if (status != RCL_RET_SERVICE_TAKE_FAILED) {
-    fprintf(stderr,
-      "[rclcpp::error] take request failed for server of service '%s': %s\n",
-      service->get_service_name().c_str(), rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "take request failed for server of service '%s': %s",
+      service->get_service_name(), rcl_get_error_string_safe());
     rcl_reset_error();
   }
 }
@@ -344,15 +376,16 @@ Executor::execute_client(
   auto request_header = client->create_request_header();
   std::shared_ptr<void> response = client->create_response();
   rcl_ret_t status = rcl_take_response(
-    client->get_client_handle(),
+    client->get_client_handle().get(),
     request_header.get(),
     response.get());
   if (status == RCL_RET_OK) {
     client->handle_response(request_header, response);
   } else if (status != RCL_RET_CLIENT_TAKE_FAILED) {
-    fprintf(stderr,
-      "[rclcpp::error] take response failed for client of service '%s': %s\n",
-      client->get_service_name().c_str(), rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "take response failed for client of service '%s': %s",
+      client->get_service_name(), rcl_get_error_string_safe());
     rcl_reset_error();
   }
 }
@@ -376,6 +409,22 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
       )
     );
   }
+  // clear wait set
+  if (rcl_wait_set_clear_subscriptions(&wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear subscriptions from wait set");
+  }
+  if (rcl_wait_set_clear_services(&wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear servicess from wait set");
+  }
+  if (rcl_wait_set_clear_clients(&wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear clients from wait set");
+  }
+  if (rcl_wait_set_clear_guard_conditions(&wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear guard conditions from wait set");
+  }
+  if (rcl_wait_set_clear_timers(&wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear timers from wait set");
+  }
 
   if (rcl_wait_set_resize_subscriptions(
       &wait_set_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK)
@@ -423,7 +472,9 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   rcl_ret_t status =
     rcl_wait(&wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
   if (status == RCL_RET_WAIT_SET_EMPTY) {
-    fprintf(stderr, "Warning: empty wait set received in rcl_wait(). This should never happen.\n");
+    RCUTILS_LOG_WARN_NAMED(
+      "rclcpp",
+      "empty wait set received in rcl_wait(). This should never happen.");
   } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
     using rclcpp::exceptions::throw_from_rcl_error;
     throw_from_rcl_error(status, "rcl_wait() failed");
@@ -432,21 +483,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   // check the null handles in the wait set and remove them from the handles in memory strategy
   // for callback-based entities
   memory_strategy_->remove_null_handles(&wait_set_);
-  if (rcl_wait_set_clear_subscriptions(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear subscriptions from wait set");
-  }
-  if (rcl_wait_set_clear_services(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear servicess from wait set");
-  }
-  if (rcl_wait_set_clear_clients(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear clients from wait set");
-  }
-  if (rcl_wait_set_clear_guard_conditions(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear guard conditions from wait set");
-  }
-  if (rcl_wait_set_clear_timers(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear timers from wait set");
-  }
 }
 
 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
@@ -495,7 +531,7 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
 }
 
 void
-Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
+Executor::get_next_timer(AnyExecutable & any_exec)
 {
   for (auto & weak_node : weak_nodes_) {
     auto node = weak_node.lock();
@@ -510,8 +546,8 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
       for (auto & timer_ref : group->get_timer_ptrs()) {
         auto timer = timer_ref.lock();
         if (timer && timer->is_ready()) {
-          any_exec->timer = timer;
-          any_exec->callback_group = group;
+          any_exec.timer = timer;
+          any_exec.callback_group = group;
           node = get_node_by_group(group);
           return;
         }
@@ -520,67 +556,69 @@ Executor::get_next_timer(AnyExecutable::SharedPtr any_exec)
   }
 }
 
-AnyExecutable::SharedPtr
-Executor::get_next_ready_executable()
+bool
+Executor::get_next_ready_executable(AnyExecutable & any_executable)
 {
-  auto any_exec = memory_strategy_->instantiate_next_executable();
   // Check the timers to see if there are any that are ready, if so return
-  get_next_timer(any_exec);
-  if (any_exec->timer) {
-    return any_exec;
+  get_next_timer(any_executable);
+  if (any_executable.timer) {
+    return true;
   }
   // Check the subscriptions to see if there are any that are ready
-  memory_strategy_->get_next_subscription(any_exec, weak_nodes_);
-  if (any_exec->subscription || any_exec->subscription_intra_process) {
-    return any_exec;
+  memory_strategy_->get_next_subscription(any_executable, weak_nodes_);
+  if (any_executable.subscription || any_executable.subscription_intra_process) {
+    return true;
   }
   // Check the services to see if there are any that are ready
-  memory_strategy_->get_next_service(any_exec, weak_nodes_);
-  if (any_exec->service) {
-    return any_exec;
+  memory_strategy_->get_next_service(any_executable, weak_nodes_);
+  if (any_executable.service) {
+    return true;
   }
   // Check the clients to see if there are any that are ready
-  memory_strategy_->get_next_client(any_exec, weak_nodes_);
-  if (any_exec->client) {
-    return any_exec;
+  memory_strategy_->get_next_client(any_executable, weak_nodes_);
+  if (any_executable.client) {
+    return true;
   }
   // If there is no ready executable, return a null ptr
-  return nullptr;
+  return false;
 }
 
-AnyExecutable::SharedPtr
-Executor::get_next_executable(std::chrono::nanoseconds timeout)
+bool
+Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout)
 {
+  bool success = false;
   // Check to see if there are any subscriptions or timers needing service
   // TODO(wjwwood): improve run to run efficiency of this function
-  auto any_exec = get_next_ready_executable();
+  success = get_next_ready_executable(any_executable);
   // If there are none
-  if (!any_exec) {
+  if (!success) {
     // Wait for subscriptions or timers to work on
     wait_for_work(timeout);
     if (!spinning.load()) {
-      return nullptr;
+      return false;
     }
     // Try again
-    any_exec = get_next_ready_executable();
+    success = get_next_ready_executable(any_executable);
   }
   // At this point any_exec should be valid with either a valid subscription
   // or a valid timer, or it should be a null shared_ptr
-  if (any_exec) {
+  if (success) {
     // If it is valid, check to see if the group is mutually exclusive or
     // not, then mark it accordingly
-    if (any_exec->callback_group && any_exec->callback_group->type() == \
-      callback_group::CallbackGroupType::MutuallyExclusive)
+    using callback_group::CallbackGroupType;
+    if (
+      any_executable.callback_group &&
+      any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive)
     {
       // It should not have been taken otherwise
-      assert(any_exec->callback_group->can_be_taken_from().load());
+      assert(any_executable.callback_group->can_be_taken_from().load());
       // Set to false to indicate something is being run from this group
       // This is reset to true either when the any_exec is executed or when the
       // any_exec is destructued
-      any_exec->callback_group->can_be_taken_from().store(false);
+      any_executable.callback_group->can_be_taken_from().store(false);
     }
   }
-  return any_exec;
+  return success;
 }
 
 std::ostream &
@@ -590,7 +628,7 @@ rclcpp::executor::operator<<(std::ostream & os, const FutureReturnCode & future_
 }
 
 std::string
-rclcpp::executor::to_string(const FutureReturnCode & future_return_code)
+rclcpp::executor::to_string(const FutureReturnCode &future_return_code)
 {
   using enum_type = std::underlying_type<FutureReturnCode>::type;
   std::string prefix = "Unknown enum value (";

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -16,6 +16,7 @@
 
 #include <chrono>
 #include <functional>
+#include <memory>
 #include <vector>
 
 #include "rclcpp/utilities.hpp"
@@ -23,10 +24,13 @@
 
 using rclcpp::executors::MultiThreadedExecutor;
 
-MultiThreadedExecutor::MultiThreadedExecutor(const rclcpp::executor::ExecutorArgs & args)
-: executor::Executor(args)
+MultiThreadedExecutor::MultiThreadedExecutor(
+  const rclcpp::executor::ExecutorArgs & args,
+  size_t number_of_threads,
+  bool yield_before_execute)
+: executor::Executor(args), yield_before_execute_(yield_before_execute)
 {
-  number_of_threads_ = std::thread::hardware_concurrency();
+  number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
   if (number_of_threads_ == 0) {
     number_of_threads_ = 1;
   }
@@ -67,14 +71,36 @@ void
 MultiThreadedExecutor::run(size_t)
 {
   while (rclcpp::ok() && spinning.load()) {
-    executor::AnyExecutable::SharedPtr any_exec;
+    executor::AnyExecutable any_exec;
     {
       std::lock_guard<std::mutex> wait_lock(wait_mutex_);
       if (!rclcpp::ok() || !spinning.load()) {
         return;
       }
-      any_exec = get_next_executable();
+      if (!get_next_executable(any_exec)) {
+        continue;
+      }
+      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;
+        }
+        scheduled_timers_.insert(any_exec.timer);
+      }
     }
+    if (yield_before_execute_) {
+      std::this_thread::yield();
+    }
+
     execute_any_executable(any_exec);
+
+    if (any_exec.timer) {
+      std::lock_guard<std::mutex> lock(scheduled_timers_mutex_);
+      auto it = scheduled_timers_.find(any_exec.timer);
+      if (it != scheduled_timers_.end()) {
+        scheduled_timers_.erase(it);
+      }
+    }
   }
 }

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/any_executable.hpp"
 #include "rclcpp/scope_exit.hpp"
 
 using rclcpp::executors::SingleThreadedExecutor;
@@ -30,7 +31,9 @@ SingleThreadedExecutor::spin()
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
   while (rclcpp::ok() && spinning.load()) {
-    auto any_exec = get_next_executable();
-    execute_any_executable(any_exec);
+    rclcpp::executor::AnyExecutable any_executable;
+    if (get_next_executable(any_executable)) {
+      execute_any_executable(any_executable);
+    }
   }
 }

rclcpp/src/rclcpp/expand_topic_or_service_name.cpp

@@ -20,6 +20,7 @@
 #include "rcl/validate_topic_name.h"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/scope_exit.hpp"
+#include "rcutils/logging_macros.h"
 #include "rcutils/types/string_map.h"
 #include "rmw/error_handling.h"
 #include "rmw/validate_namespace.h"
@@ -61,9 +62,9 @@ rclcpp::expand_topic_or_service_name(
     // finalize the string map before throwing
     rcutils_ret = rcutils_string_map_fini(&substitutions_map);
     if (rcutils_ret != RCUTILS_RET_OK) {
-      fprintf(stderr,
-        "[rclcpp|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: "
-        "failed to fini string_map (%d) during error handling: %s\n",
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to fini string_map (%d) during error handling: %s",
         rcutils_ret,
         rcutils_get_error_string_safe());
       rcutils_reset_error();
@@ -102,20 +103,20 @@ rclcpp::expand_topic_or_service_name(
         throw_from_rcl_error(ret);
       }
 
-      if (validation_result == RCL_TOPIC_NAME_VALID) {
+      if (validation_result != RCL_TOPIC_NAME_VALID) {
+        const char * validation_message =
+          rcl_topic_name_validation_result_string(validation_result);
+        if (is_service) {
+          using rclcpp::exceptions::InvalidServiceNameError;
+          throw InvalidServiceNameError(name.c_str(), validation_message, invalid_index);
+        } else {
+          using rclcpp::exceptions::InvalidTopicNameError;
+          throw InvalidTopicNameError(name.c_str(), validation_message, invalid_index);
+        }
+      } else {
         throw std::runtime_error("topic name unexpectedly valid");
       }
-      const char * validation_message = rcl_topic_name_validation_result_string(validation_result);
-      if (!validation_message) {
-        throw std::runtime_error("unable to get validation error message");
-      }
-      if (is_service) {
-        using rclcpp::exceptions::InvalidServiceNameError;
-        throw InvalidServiceNameError(name.c_str(), validation_message, invalid_index);
-      } else {
-        using rclcpp::exceptions::InvalidTopicNameError;
-        throw InvalidTopicNameError(name.c_str(), validation_message, invalid_index);
-      }
+
       // if invalid node name
     } else if (ret == RCL_RET_NODE_INVALID_NAME) {
       rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
@@ -133,10 +134,16 @@ rclcpp::expand_topic_or_service_name(
           RCL_RET_ERROR, "failed to validate node name",
           rmw_get_error_state(), rmw_reset_error);
       }
-      throw rclcpp::exceptions::InvalidNodeNameError(
-              node_name.c_str(),
-              rmw_node_name_validation_result_string(validation_result),
-              invalid_index);
+
+      if (validation_result != RMW_NODE_NAME_VALID) {
+        throw rclcpp::exceptions::InvalidNodeNameError(
+                node_name.c_str(),
+                rmw_node_name_validation_result_string(validation_result),
+                invalid_index);
+      } else {
+        throw std::runtime_error("invalid rcl node name but valid rmw node name");
+      }
+
       // if invalid namespace
     } else if (ret == RCL_RET_NODE_INVALID_NAMESPACE) {
       rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
@@ -154,10 +161,15 @@ rclcpp::expand_topic_or_service_name(
           RCL_RET_ERROR, "failed to validate namespace",
           rmw_get_error_state(), rmw_reset_error);
       }
-      throw rclcpp::exceptions::InvalidNamespaceError(
-              namespace_.c_str(),
-              rmw_namespace_validation_result_string(validation_result),
-              invalid_index);
+
+      if (validation_result != RMW_NAMESPACE_VALID) {
+        throw rclcpp::exceptions::InvalidNamespaceError(
+                namespace_.c_str(),
+                rmw_namespace_validation_result_string(validation_result),
+                invalid_index);
+      } else {
+        throw std::runtime_error("invalid rcl namespace but valid rmw namespace");
+      }
       // something else happened
     } else {
       throw_from_rcl_error(ret);
@@ -179,6 +191,7 @@ rclcpp::expand_topic_or_service_name(
       RCL_RET_ERROR, "failed to validate full topic name",
       rmw_get_error_state(), rmw_reset_error);
   }
+
   if (validation_result != RMW_TOPIC_VALID) {
     if (is_service) {
       throw rclcpp::exceptions::InvalidServiceNameError(

rclcpp/src/rclcpp/graph_listener.cpp

@@ -26,6 +26,8 @@
 #include "rclcpp/node.hpp"
 #include "rmw/impl/cpp/demangle.hpp"
 
+#include "rcutils/logging_macros.h"
+
 using rclcpp::exceptions::throw_from_rcl_error;
 
 namespace rclcpp
@@ -94,13 +96,16 @@ GraphListener::run()
   try {
     run_loop();
   } catch (const std::exception & exc) {
-    fprintf(stderr,
-      "[rclcpp] caught %s exception in GraphListener thread: %s\n",
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "caught %s exception in GraphListener thread: %s",
       rmw::impl::cpp::demangle(exc).c_str(),
       exc.what());
     std::rethrow_exception(std::current_exception());
   } catch (...) {
-    fprintf(stderr, "[rclcpp] unknown error in GraphListener thread\n");
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "unknown error in GraphListener thread");
     std::rethrow_exception(std::current_exception());
   }
 }

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -13,12 +13,14 @@
 // limitations under the License.
 
 #include "rclcpp/memory_strategy.hpp"
+#include <memory>
 
 using rclcpp::memory_strategy::MemoryStrategy;
 
 rclcpp::SubscriptionBase::SharedPtr
 MemoryStrategy::get_subscription_by_handle(
-  const rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes)
+  std::shared_ptr<const rcl_subscription_t> subscriber_handle,
+  const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
     auto node = weak_node.lock();
@@ -48,7 +50,7 @@ MemoryStrategy::get_subscription_by_handle(
 
 rclcpp::ServiceBase::SharedPtr
 MemoryStrategy::get_service_by_handle(
-  const rcl_service_t * service_handle,
+  std::shared_ptr<const rcl_service_t> service_handle,
   const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {
@@ -74,7 +76,7 @@ MemoryStrategy::get_service_by_handle(
 
 rclcpp::ClientBase::SharedPtr
 MemoryStrategy::get_client_by_handle(
-  const rcl_client_t * client_handle,
+  std::shared_ptr<const rcl_client_t> client_handle,
   const WeakNodeVector & weak_nodes)
 {
   for (auto & weak_node : weak_nodes) {

rclcpp/src/rclcpp/node.cpp

@@ -43,23 +43,36 @@ Node::Node(
     node_name,
     namespace_,
     rclcpp::contexts::default_context::get_global_default_context(),
-    use_intra_process_comms)
+    {},
+    {},
+    true,
+    use_intra_process_comms,
+    true)
 {}
 
 Node::Node(
   const std::string & node_name,
   const std::string & namespace_,
   rclcpp::Context::SharedPtr context,
-  bool use_intra_process_comms)
-: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, 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)
+: node_base_(new rclcpp::node_interfaces::NodeBase(
+      node_name, namespace_, context, arguments, use_global_arguments)),
   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())),
   node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
   node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
   node_parameters_(new rclcpp::node_interfaces::NodeParameters(
-      node_topics_.get(),
-      use_intra_process_comms
+      node_base_,
+      node_topics_,
+      node_services_,
+      initial_parameters,
+      use_intra_process_comms,
+      start_parameter_services
     )),
   node_clock_(new rclcpp::node_interfaces::NodeClock(
       node_base_,
@@ -107,26 +120,26 @@ Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group)
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
 Node::set_parameters(
-  const std::vector<rclcpp::parameter::ParameterVariant> & 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::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   return node_parameters_->set_parameters_atomically(parameters);
 }
 
-std::vector<rclcpp::parameter::ParameterVariant>
+std::vector<rclcpp::Parameter>
 Node::get_parameters(
   const std::vector<std::string> & names) const
 {
   return node_parameters_->get_parameters(names);
 }
 
-rclcpp::parameter::ParameterVariant
+rclcpp::Parameter
 Node::get_parameter(const std::string & name) const
 {
   return node_parameters_->get_parameter(name);
@@ -134,7 +147,7 @@ Node::get_parameter(const std::string & name) const
 
 bool Node::get_parameter(
   const std::string & name,
-  rclcpp::parameter::ParameterVariant & parameter) const
+  rclcpp::Parameter & parameter) const
 {
   return node_parameters_->get_parameter(name, parameter);
 }

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -19,7 +19,9 @@
 
 #include "rclcpp/node_interfaces/node_base.hpp"
 
+#include "rcl/arguments.h"
 #include "rclcpp/exceptions.hpp"
+#include "rcutils/logging_macros.h"
 #include "rmw/validate_node_name.h"
 #include "rmw/validate_namespace.h"
 
@@ -30,7 +32,9 @@ using rclcpp::node_interfaces::NodeBase;
 NodeBase::NodeBase(
   const std::string & node_name,
   const std::string & namespace_,
-  rclcpp::Context::SharedPtr context)
+  rclcpp::Context::SharedPtr context,
+  const std::vector<std::string> & arguments,
+  bool use_global_arguments)
 : context_(context),
   node_handle_(nullptr),
   default_callback_group_(nullptr),
@@ -48,8 +52,9 @@ NodeBase::NodeBase(
   auto finalize_notify_guard_condition = [this]() {
       // Finalize the interrupt guard condition.
       if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
-        fprintf(stderr,
-          "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "failed to destroy guard condition: %s", rcl_get_error_string_safe());
       }
     };
 
@@ -82,17 +87,44 @@ NodeBase::NodeBase(
   }
 
   // Create the rcl node and store it in a shared_ptr with a custom destructor.
-  rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node());
+  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, node_name.c_str(), namespace_.c_str(), &options);
+
+  ret = rcl_node_init(rcl_node.get(), node_name.c_str(), namespace_.c_str(), &options);
   if (ret != RCL_RET_OK) {
     // Finalize the interrupt guard condition.
     finalize_notify_guard_condition();
-
-    delete rcl_node;
+    // 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_safe());
+      rcl_reset_error();
+    }
 
     if (ret == RCL_RET_NODE_INVALID_NAME) {
       rcl_reset_error();  // discard rcl_node_init error
@@ -106,10 +138,15 @@ NodeBase::NodeBase(
         }
         throw_from_rcl_error(RCL_RET_ERROR, "failed to validate node name");
       }
-      throw rclcpp::exceptions::InvalidNodeNameError(
-              node_name.c_str(),
-              rmw_node_name_validation_result_string(validation_result),
-              invalid_index);
+
+      if (validation_result != RMW_NODE_NAME_VALID) {
+        throw rclcpp::exceptions::InvalidNodeNameError(
+                node_name.c_str(),
+                rmw_node_name_validation_result_string(validation_result),
+                invalid_index);
+      } else {
+        throw std::runtime_error("valid rmw node name but invalid rcl node name");
+      }
     }
 
     if (ret == RCL_RET_NODE_INVALID_NAMESPACE) {
@@ -124,21 +161,26 @@ NodeBase::NodeBase(
         }
         throw_from_rcl_error(RCL_RET_ERROR, "failed to validate namespace");
       }
-      throw rclcpp::exceptions::InvalidNamespaceError(
-              namespace_.c_str(),
-              rmw_namespace_validation_result_string(validation_result),
-              invalid_index);
-    }
 
+      if (validation_result != RMW_NAMESPACE_VALID) {
+        throw rclcpp::exceptions::InvalidNamespaceError(
+                namespace_.c_str(),
+                rmw_namespace_validation_result_string(validation_result),
+                invalid_index);
+      } else {
+        throw std::runtime_error("valid rmw node namespace but invalid rcl node namespace");
+      }
+    }
     throw_from_rcl_error(ret, "failed to initialize rcl node");
   }
 
   node_handle_.reset(
-    rcl_node,
+    rcl_node.release(),
     [](rcl_node_t * node) -> void {
       if (rcl_node_fini(node) != RCL_RET_OK) {
-        fprintf(
-          stderr, "Error in destruction of rcl node handle: %s\n", rcl_get_error_string_safe());
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Error in destruction of rcl node handle: %s", rcl_get_error_string_safe());
       }
       delete node;
     });
@@ -149,6 +191,15 @@ 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_safe());
+    rcl_reset_error();
+  }
 }
 
 NodeBase::~NodeBase()
@@ -158,8 +209,9 @@ NodeBase::~NodeBase()
     std::lock_guard<std::recursive_mutex> notify_condition_lock(notify_guard_condition_mutex_);
     notify_guard_condition_is_valid_ = false;
     if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
-      fprintf(stderr,
-        "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to destroy guard condition: %s", rcl_get_error_string_safe());
     }
   }
 }

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -149,8 +149,12 @@ NodeGraph::get_node_names() const
     throw std::runtime_error(error_msg);
   }
 
-  std::vector<std::string> node_names(&node_names_c.data[0],
-    &node_names_c.data[0 + node_names_c.size]);
+  std::vector<std::string> node_names(node_names_c.size);
+  for (size_t i = 0; i < node_names_c.size; ++i) {
+    if (node_names_c.data[i]) {
+      node_names[i] = node_names_c.data[i];
+    }
+  }
   ret = rcutils_string_array_fini(&node_names_c);
   if (ret != RCUTILS_RET_OK) {
     // *INDENT-OFF*
@@ -166,16 +170,16 @@ NodeGraph::get_node_names() const
 size_t
 NodeGraph::count_publishers(const std::string & topic_name) const
 {
-  auto rmw_node_handle = rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle());
+  auto rcl_node_handle = node_base_->get_rcl_node_handle();
+
   auto fqdn = rclcpp::expand_topic_or_service_name(
     topic_name,
-    rmw_node_handle->name,
-    rmw_node_handle->namespace_,
+    rcl_node_get_name(rcl_node_handle),
+    rcl_node_get_namespace(rcl_node_handle),
     false);    // false = not a service
 
   size_t count;
-  // TODO(wjwwood): use the rcl equivalent methods
-  auto ret = rmw_count_publishers(rmw_node_handle, fqdn.c_str(), &count);
+  auto ret = rcl_count_publishers(rcl_node_handle, fqdn.c_str(), &count);
   if (ret != RMW_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
@@ -188,16 +192,16 @@ NodeGraph::count_publishers(const std::string & topic_name) const
 size_t
 NodeGraph::count_subscribers(const std::string & topic_name) const
 {
-  auto rmw_node_handle = rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle());
+  auto rcl_node_handle = node_base_->get_rcl_node_handle();
+
   auto fqdn = rclcpp::expand_topic_or_service_name(
     topic_name,
-    rmw_node_handle->name,
-    rmw_node_handle->namespace_,
+    rcl_node_get_name(rcl_node_handle),
+    rcl_node_get_namespace(rcl_node_handle),
     false);    // false = not a service
 
   size_t count;
-  // TODO(wjwwood): use the rcl equivalent methods
-  auto ret = rmw_count_subscribers(rmw_node_handle, fqdn.c_str(), &count);
+  auto ret = rcl_count_subscribers(rcl_node_handle, fqdn.c_str(), &count);
   if (ret != RMW_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(

rclcpp/src/rclcpp/node_interfaces/node_logging.cpp

@@ -19,8 +19,7 @@ using rclcpp::node_interfaces::NodeLogging;
 NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base)
 : node_base_(node_base)
 {
-  // TODO(dhood): use the namespace (slashes converted to dots)
-  logger_ = rclcpp::get_logger(node_base_->get_name());
+  logger_ = rclcpp::get_logger(this->get_logger_name());
 }
 
 NodeLogging::~NodeLogging()
@@ -32,3 +31,9 @@ NodeLogging::get_logger() const
 {
   return logger_;
 }
+
+const char *
+NodeLogging::get_logger_name() const
+{
+  return rcl_node_get_logger_name(node_base_->get_rcl_node_handle());
+}

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -14,6 +14,8 @@
 
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 
+#include <rcl_yaml_param_parser/parser.h>
+
 #include <map>
 #include <memory>
 #include <string>
@@ -22,15 +24,20 @@
 
 #include "rcl_interfaces/srv/list_parameters.hpp"
 #include "rclcpp/create_publisher.hpp"
+#include "rclcpp/parameter_map.hpp"
+#include "rclcpp/scope_exit.hpp"
 #include "rcutils/logging_macros.h"
 #include "rmw/qos_profiles.h"
 
 using rclcpp::node_interfaces::NodeParameters;
 
 NodeParameters::NodeParameters(
-  rclcpp::node_interfaces::NodeTopicsInterface * node_topics,
-  bool use_intra_process)
-: node_topics_(node_topics)
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+  const std::vector<rclcpp::Parameter> & initial_parameters,
+  bool use_intra_process,
+  bool start_parameter_services)
 {
   using MessageT = rcl_interfaces::msg::ParameterEvent;
   using PublisherT = rclcpp::Publisher<MessageT>;
@@ -38,12 +45,115 @@ NodeParameters::NodeParameters(
   // TODO(wjwwood): expose this allocator through the Parameter interface.
   auto allocator = std::make_shared<AllocatorT>();
 
+  if (start_parameter_services) {
+    parameter_service_ = std::make_shared<ParameterService>(node_base, node_services, this);
+  }
+
   events_publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
-    node_topics_,
+    node_topics.get(),
     "parameter_events",
     rmw_qos_profile_parameter_events,
     use_intra_process,
     allocator);
+
+  // Get the node options
+  const rcl_node_t * node = node_base->get_rcl_node_handle();
+  if (nullptr == node) {
+    throw std::runtime_error("Need valid node handle in NodeParameters");
+  }
+  const rcl_node_options_t * options = rcl_node_get_options(node);
+  if (nullptr == options) {
+    throw std::runtime_error("Need valid node options NodeParameters");
+  }
+
+  // Get paths to yaml files containing initial parameter values
+  std::vector<std::string> yaml_paths;
+
+  auto get_yaml_paths = [&yaml_paths, &options](const rcl_arguments_t * args) {
+      int num_yaml_files = rcl_arguments_get_param_files_count(args);
+      if (num_yaml_files > 0) {
+        char ** param_files;
+        rcl_ret_t ret = rcl_arguments_get_param_files(args, options->allocator, &param_files);
+        if (RCL_RET_OK != ret) {
+          rclcpp::exceptions::throw_from_rcl_error(ret);
+        }
+        auto cleanup_param_files = make_scope_exit(
+          [&param_files, &num_yaml_files, &options]() {
+            for (int i = 0; i < num_yaml_files; ++i) {
+              options->allocator.deallocate(param_files[i], options->allocator.state);
+            }
+            options->allocator.deallocate(param_files, options->allocator.state);
+          });
+        for (int i = 0; i < num_yaml_files; ++i) {
+          yaml_paths.emplace_back(param_files[i]);
+        }
+      }
+    };
+
+  // global before local so that local overwrites global
+  if (options->use_global_arguments) {
+    get_yaml_paths(rcl_get_global_arguments());
+  }
+  get_yaml_paths(&(options->arguments));
+
+  // Get fully qualified node name post-remapping to use to find node's params in yaml files
+  const std::string node_name = node_base->get_name();
+  const std::string node_namespace = node_base->get_namespace();
+  if (0u == node_namespace.size() || 0u == node_name.size()) {
+    // Should never happen
+    throw std::runtime_error("Node name and namespace were not set");
+  }
+  std::string combined_name;
+  if ('/' == node_namespace.at(node_namespace.size() - 1)) {
+    combined_name = node_namespace + node_name;
+  } else {
+    combined_name = node_namespace + '/' + node_name;
+  }
+
+  std::map<std::string, rclcpp::Parameter> parameters;
+
+  // TODO(sloretz) use rcl to parse yaml when circular dependency is solved
+  for (const std::string & yaml_path : yaml_paths) {
+    rcl_params_t * yaml_params = rcl_yaml_node_struct_init(options->allocator);
+    if (nullptr == yaml_params) {
+      throw std::bad_alloc();
+    }
+    if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params)) {
+      throw std::runtime_error("Failed to parse parameters " + yaml_path);
+    }
+
+    rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(yaml_params);
+    rcl_yaml_node_struct_fini(yaml_params);
+    auto iter = initial_map.find(combined_name);
+    if (initial_map.end() == iter) {
+      continue;
+    }
+
+    // Combine parameter yaml files, overwriting values in older ones
+    for (auto & param : iter->second) {
+      parameters[param.get_name()] = param;
+    }
+  }
+
+  // initial values passed to constructor overwrite yaml file sources
+  for (auto & param : initial_parameters) {
+    parameters[param.get_name()] = param;
+  }
+
+  std::vector<rclcpp::Parameter> combined_values;
+  combined_values.reserve(parameters.size());
+  for (auto & kv : parameters) {
+    combined_values.emplace_back(kv.second);
+  }
+
+  // TODO(sloretz) store initial values and use them when a parameter is created ros2/rclcpp#475
+  // Set initial parameter values
+  if (!combined_values.empty()) {
+    rcl_interfaces::msg::SetParametersResult result = set_parameters_atomically(combined_values);
+    if (!result.successful) {
+      throw std::runtime_error("Failed to set initial parameters");
+    }
+  }
 }
 
 NodeParameters::~NodeParameters()
@@ -51,7 +161,7 @@ NodeParameters::~NodeParameters()
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
 NodeParameters::set_parameters(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   std::vector<rcl_interfaces::msg::SetParametersResult> results;
   for (auto p : parameters) {
@@ -63,10 +173,10 @@ NodeParameters::set_parameters(
 
 rcl_interfaces::msg::SetParametersResult
 NodeParameters::set_parameters_atomically(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
+  std::map<std::string, rclcpp::Parameter> tmp_map;
   auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
 
   // TODO(jacquelinekay): handle parameter constraints
@@ -82,27 +192,34 @@ NodeParameters::set_parameters_atomically(
   }
 
   for (auto p : parameters) {
-    if (parameters_.find(p.get_name()) == parameters_.end()) {
-      if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
-        // case: parameter not set before, and input is something other than "NOT_SET"
-        parameter_event->new_parameters.push_back(p.to_parameter());
+    if (p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      if (parameters_.find(p.get_name()) != parameters_.end()) {
+        // case: parameter was set before, and input is "NOT_SET"
+        // therefore we will erase the parameter from parameters_ later
+        parameter_event->deleted_parameters.push_back(p.to_parameter_msg());
       }
-    } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
-      // case: parameter was set before, and input is something other than "NOT_SET"
-      parameter_event->changed_parameters.push_back(p.to_parameter());
     } else {
-      // case: parameter was set before, and input is "NOT_SET"
-      // therefore we will "unset" the previously set parameter
-      // it is not necessary to erase the parameter from parameters_
-      // because the new value for this key (p.get_name()) will be a
-      // ParameterVariant with type "NOT_SET"
-      parameter_event->deleted_parameters.push_back(p.to_parameter());
+      if (parameters_.find(p.get_name()) == parameters_.end()) {
+        // case: parameter not set before, and input is something other than "NOT_SET"
+        parameter_event->new_parameters.push_back(p.to_parameter_msg());
+      } else {
+        // case: parameter was set before, and input is something other than "NOT_SET"
+        parameter_event->changed_parameters.push_back(p.to_parameter_msg());
+      }
+      tmp_map[p.get_name()] = p;
     }
-    tmp_map[p.get_name()] = p;
   }
   // 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 explicitly deleted parameters
+  for (auto p : parameters) {
+    if (p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+      tmp_map.erase(p.get_name());
+    }
+  }
+
   std::swap(tmp_map, parameters_);
 
   events_publisher_->publish(parameter_event);
@@ -110,15 +227,15 @@ NodeParameters::set_parameters_atomically(
   return result;
 }
 
-std::vector<rclcpp::parameter::ParameterVariant>
+std::vector<rclcpp::Parameter>
 NodeParameters::get_parameters(const std::vector<std::string> & names) const
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  std::vector<rclcpp::parameter::ParameterVariant> results;
+  std::vector<rclcpp::Parameter> results;
 
   for (auto & name : names) {
     if (std::any_of(parameters_.cbegin(), parameters_.cend(),
-      [&name](const std::pair<std::string, rclcpp::parameter::ParameterVariant> & kv) {
+      [&name](const std::pair<std::string, rclcpp::Parameter> & kv) {
         return name == kv.first;
       }))
     {
@@ -128,10 +245,10 @@ NodeParameters::get_parameters(const std::vector<std::string> & names) const
   return results;
 }
 
-rclcpp::parameter::ParameterVariant
+rclcpp::Parameter
 NodeParameters::get_parameter(const std::string & name) const
 {
-  rclcpp::parameter::ParameterVariant parameter;
+  rclcpp::Parameter parameter;
 
   if (get_parameter(name, parameter)) {
     return parameter;
@@ -143,7 +260,7 @@ NodeParameters::get_parameter(const std::string & name) const
 bool
 NodeParameters::get_parameter(
   const std::string & name,
-  rclcpp::parameter::ParameterVariant & parameter) const
+  rclcpp::Parameter & parameter) const
 {
   std::lock_guard<std::mutex> lock(mutex_);
 

rclcpp/src/rclcpp/parameter.cpp

@@ -20,214 +20,120 @@
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/utilities.hpp"
 
-using rclcpp::parameter::ParameterType;
-using rclcpp::parameter::ParameterVariant;
+using rclcpp::ParameterType;
+using rclcpp::Parameter;
 
-ParameterVariant::ParameterVariant()
+Parameter::Parameter()
 : name_("")
 {
-  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET;
 }
 
-ParameterVariant::ParameterVariant(const std::string & name, const bool bool_value)
-: name_(name)
+Parameter::Parameter(const std::string & name, const rclcpp::ParameterValue & value)
+: name_(name), value_(value)
 {
-  value_.bool_value = bool_value;
-  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
-}
-
-ParameterVariant::ParameterVariant(const std::string & name, const int int_value)
-: name_(name)
-{
-  value_.integer_value = int_value;
-  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
-}
-
-ParameterVariant::ParameterVariant(const std::string & name, const int64_t int_value)
-: name_(name)
-{
-  value_.integer_value = int_value;
-  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
-}
-
-ParameterVariant::ParameterVariant(const std::string & name, const float double_value)
-: name_(name)
-{
-  value_.double_value = double_value;
-  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-}
-
-ParameterVariant::ParameterVariant(const std::string & name, const double double_value)
-: name_(name)
-{
-  value_.double_value = double_value;
-  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
-}
-
-ParameterVariant::ParameterVariant(const std::string & name, const std::string & string_value)
-: name_(name)
-{
-  value_.string_value = string_value;
-  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
-}
-
-ParameterVariant::ParameterVariant(const std::string & name, const char * string_value)
-: ParameterVariant(name, std::string(string_value))
-{}
-
-ParameterVariant::ParameterVariant(
-  const std::string & name, const std::vector<uint8_t> & bytes_value)
-: name_(name)
-{
-  value_.bytes_value = bytes_value;
-  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES;
 }
 
 ParameterType
-ParameterVariant::get_type() const
+Parameter::get_type() const
 {
-  return static_cast<ParameterType>(value_.type);
+  return value_.get_type();
 }
 
 std::string
-ParameterVariant::get_type_name() const
+Parameter::get_type_name() const
 {
-  switch (get_type()) {
-    case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
-      return "bool";
-    case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
-      return "integer";
-    case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
-      return "double";
-    case rclcpp::parameter::ParameterType::PARAMETER_STRING:
-      return "string";
-    case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
-      return "bytes";
-    case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
-      return "not set";
-    default:
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
-      // *INDENT-ON*
-  }
+  return rclcpp::to_string(get_type());
 }
 
 const std::string &
-ParameterVariant::get_name() const
+Parameter::get_name() const
 {
   return name_;
 }
 
 rcl_interfaces::msg::ParameterValue
-ParameterVariant::get_parameter_value() const
+Parameter::get_value_message() const
 {
-  return value_;
+  return value_.to_value_msg();
+}
+
+bool
+Parameter::as_bool() const
+{
+  return get_value<ParameterType::PARAMETER_BOOL>();
 }
 
 int64_t
-ParameterVariant::as_int() const
+Parameter::as_int() const
 {
   return get_value<ParameterType::PARAMETER_INTEGER>();
 }
 
 double
-ParameterVariant::as_double() const
+Parameter::as_double() const
 {
   return get_value<ParameterType::PARAMETER_DOUBLE>();
 }
 
 const std::string &
-ParameterVariant::as_string() const
+Parameter::as_string() const
 {
   return get_value<ParameterType::PARAMETER_STRING>();
 }
 
-bool
-ParameterVariant::as_bool() const
-{
-  return get_value<ParameterType::PARAMETER_BOOL>();
-}
-
 const std::vector<uint8_t> &
-ParameterVariant::as_bytes() const
+Parameter::as_byte_array() const
 {
-  return get_value<ParameterType::PARAMETER_BYTES>();
+  return get_value<ParameterType::PARAMETER_BYTE_ARRAY>();
 }
 
-ParameterVariant
-ParameterVariant::from_parameter(const rcl_interfaces::msg::Parameter & parameter)
+const std::vector<bool> &
+Parameter::as_bool_array() const
 {
-  switch (parameter.value.type) {
-    case PARAMETER_BOOL:
-      return ParameterVariant(parameter.name, parameter.value.bool_value);
-    case PARAMETER_INTEGER:
-      return ParameterVariant(parameter.name, parameter.value.integer_value);
-    case PARAMETER_DOUBLE:
-      return ParameterVariant(parameter.name, parameter.value.double_value);
-    case PARAMETER_STRING:
-      return ParameterVariant(parameter.name, parameter.value.string_value);
-    case PARAMETER_BYTES:
-      return ParameterVariant(parameter.name, parameter.value.bytes_value);
-    case PARAMETER_NOT_SET:
-      throw std::runtime_error("Type from ParameterValue is not set");
-    default:
-      // TODO(wjwwood): use custom exception
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        "Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type));
-      // *INDENT-ON*
-  }
+  return get_value<ParameterType::PARAMETER_BOOL_ARRAY>();
+}
+
+const std::vector<int64_t> &
+Parameter::as_integer_array() const
+{
+  return get_value<ParameterType::PARAMETER_INTEGER_ARRAY>();
+}
+
+const std::vector<double> &
+Parameter::as_double_array() const
+{
+  return get_value<ParameterType::PARAMETER_DOUBLE_ARRAY>();
+}
+
+const std::vector<std::string> &
+Parameter::as_string_array() const
+{
+  return get_value<ParameterType::PARAMETER_STRING_ARRAY>();
+}
+
+Parameter
+Parameter::from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter)
+{
+  return Parameter(parameter.name, parameter.value);
 }
 
 rcl_interfaces::msg::Parameter
-ParameterVariant::to_parameter()
+Parameter::to_parameter_msg() const
 {
   rcl_interfaces::msg::Parameter parameter;
   parameter.name = name_;
-  parameter.value = value_;
+  parameter.value = value_.to_value_msg();
   return parameter;
 }
 
 std::string
-ParameterVariant::value_to_string() const
+Parameter::value_to_string() const
 {
-  switch (get_type()) {
-    case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
-      return as_bool() ? "true" : "false";
-    case rclcpp::parameter::ParameterType::PARAMETER_INTEGER:
-      return std::to_string(as_int());
-    case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE:
-      return std::to_string(as_double());
-    case rclcpp::parameter::ParameterType::PARAMETER_STRING:
-      return as_string();
-    case rclcpp::parameter::ParameterType::PARAMETER_BYTES:
-      {
-        std::stringstream bytes;
-        bool first_byte = true;
-        bytes << "[" << std::hex;
-        for (auto & byte : as_bytes()) {
-          bytes << "0x" << byte;
-          if (!first_byte) {
-            bytes << ", ";
-          } else {
-            first_byte = false;
-          }
-        }
-        return bytes.str();
-      }
-    case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET:
-      return "not set";
-    default:
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        "Unexpected type from ParameterVariant: " + std::to_string(get_type()));
-      // *INDENT-ON*
-  }
+  return rclcpp::to_string(value_);
 }
 
 std::string
-rclcpp::parameter::_to_json_dict_entry(const ParameterVariant & param)
+rclcpp::_to_json_dict_entry(const Parameter & param)
 {
   std::stringstream ss;
   ss << "\"" << param.get_name() << "\": ";
@@ -237,21 +143,21 @@ rclcpp::parameter::_to_json_dict_entry(const ParameterVariant & param)
 }
 
 std::ostream &
-rclcpp::parameter::operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv)
+rclcpp::operator<<(std::ostream & os, const rclcpp::Parameter & pv)
 {
   os << std::to_string(pv);
   return os;
 }
 
 std::ostream &
-rclcpp::parameter::operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters)
+rclcpp::operator<<(std::ostream & os, const std::vector<Parameter> & parameters)
 {
   os << std::to_string(parameters);
   return os;
 }
 
 std::string
-std::to_string(const rclcpp::parameter::ParameterVariant & param)
+std::to_string(const rclcpp::Parameter & param)
 {
   std::stringstream ss;
   ss << "{\"name\": \"" << param.get_name() << "\", ";
@@ -261,7 +167,7 @@ std::to_string(const rclcpp::parameter::ParameterVariant & param)
 }
 
 std::string
-std::to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+std::to_string(const std::vector<rclcpp::Parameter> & parameters)
 {
   std::stringstream ss;
   ss << "{";
@@ -272,7 +178,7 @@ std::to_string(const std::vector<rclcpp::parameter::ParameterVariant> & paramete
     } else {
       first = false;
     }
-    ss << rclcpp::parameter::_to_json_dict_entry(pv);
+    ss << rclcpp::_to_json_dict_entry(pv);
   }
   ss << "}";
   return ss.str();

rclcpp/src/rclcpp/parameter_client.cpp

@@ -70,6 +70,15 @@ AsyncParametersClient::AsyncParametersClient(
   auto set_parameters_base = std::dynamic_pointer_cast<ClientBase>(set_parameters_client_);
   node_services_interface->add_client(set_parameters_base, nullptr);
 
+  set_parameters_atomically_client_ =
+    Client<rcl_interfaces::srv::SetParametersAtomically>::make_shared(node_base_interface.get(),
+      node_graph_interface,
+      remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically,
+      options);
+  auto set_parameters_atomically_base = std::dynamic_pointer_cast<ClientBase>(
+    set_parameters_atomically_client_);
+  node_services_interface->add_client(set_parameters_atomically_base, nullptr);
+
   list_parameters_client_ = Client<rcl_interfaces::srv::ListParameters>::make_shared(
     node_base_interface.get(),
     node_graph_interface,
@@ -114,15 +123,15 @@ AsyncParametersClient::AsyncParametersClient(
     qos_profile)
 {}
 
-std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
+std::shared_future<std::vector<rclcpp::Parameter>>
 AsyncParametersClient::get_parameters(
   const std::vector<std::string> & names,
   std::function<
-    void(std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)
+    void(std::shared_future<std::vector<rclcpp::Parameter>>)
   > callback)
 {
   auto promise_result =
-    std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterVariant>>>();
+    std::make_shared<std::promise<std::vector<rclcpp::Parameter>>>();
   auto future_result = promise_result->get_future().share();
 
   auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
@@ -133,7 +142,7 @@ AsyncParametersClient::get_parameters(
     [request, promise_result, future_result, callback](
       rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedFuture cb_f)
     {
-      std::vector<rclcpp::parameter::ParameterVariant> parameter_variants;
+      std::vector<rclcpp::Parameter> parameters;
       auto & pvalues = cb_f.get()->values;
 
       for (auto & pvalue : pvalues) {
@@ -141,11 +150,11 @@ AsyncParametersClient::get_parameters(
         rcl_interfaces::msg::Parameter parameter;
         parameter.name = request->names[i];
         parameter.value = pvalue;
-        parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(
+        parameters.push_back(rclcpp::Parameter::from_parameter_msg(
           parameter));
       }
 
-      promise_result->set_value(parameter_variants);
+      promise_result->set_value(parameters);
       if (callback != nullptr) {
         callback(future_result);
       }
@@ -155,15 +164,15 @@ AsyncParametersClient::get_parameters(
   return future_result;
 }
 
-std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
+std::shared_future<std::vector<rclcpp::ParameterType>>
 AsyncParametersClient::get_parameter_types(
   const std::vector<std::string> & names,
   std::function<
-    void(std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)
+    void(std::shared_future<std::vector<rclcpp::ParameterType>>)
   > callback)
 {
   auto promise_result =
-    std::make_shared<std::promise<std::vector<rclcpp::parameter::ParameterType>>>();
+    std::make_shared<std::promise<std::vector<rclcpp::ParameterType>>>();
   auto future_result = promise_result->get_future().share();
 
   auto request = std::make_shared<rcl_interfaces::srv::GetParameterTypes::Request>();
@@ -174,10 +183,10 @@ AsyncParametersClient::get_parameter_types(
     [promise_result, future_result, callback](
       rclcpp::Client<rcl_interfaces::srv::GetParameterTypes>::SharedFuture cb_f)
     {
-      std::vector<rclcpp::parameter::ParameterType> types;
+      std::vector<rclcpp::ParameterType> types;
       auto & pts = cb_f.get()->types;
       for (auto & pt : pts) {
-        pts.push_back(static_cast<rclcpp::parameter::ParameterType>(pt));
+        pts.push_back(static_cast<rclcpp::ParameterType>(pt));
       }
       promise_result->set_value(types);
       if (callback != nullptr) {
@@ -191,7 +200,7 @@ AsyncParametersClient::get_parameter_types(
 
 std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
 AsyncParametersClient::set_parameters(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
+  const std::vector<rclcpp::Parameter> & parameters,
   std::function<
     void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
   > callback)
@@ -203,8 +212,8 @@ AsyncParametersClient::set_parameters(
   auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
 
   std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
-    [](rclcpp::parameter::ParameterVariant p) {
-      return p.to_parameter();
+    [](rclcpp::Parameter p) {
+      return p.to_parameter_msg();
     }
   );
 
@@ -225,7 +234,7 @@ AsyncParametersClient::set_parameters(
 
 std::shared_future<rcl_interfaces::msg::SetParametersResult>
 AsyncParametersClient::set_parameters_atomically(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
+  const std::vector<rclcpp::Parameter> & parameters,
   std::function<
     void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)
   > callback)
@@ -237,8 +246,8 @@ AsyncParametersClient::set_parameters_atomically(
   auto request = std::make_shared<rcl_interfaces::srv::SetParametersAtomically::Request>();
 
   std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters),
-    [](rclcpp::parameter::ParameterVariant p) {
-      return p.to_parameter();
+    [](rclcpp::Parameter p) {
+      return p.to_parameter_msg();
     }
   );
 
@@ -347,7 +356,7 @@ SyncParametersClient::SyncParametersClient(
     std::make_shared<AsyncParametersClient>(node, remote_node_name, qos_profile);
 }
 
-std::vector<rclcpp::parameter::ParameterVariant>
+std::vector<rclcpp::Parameter>
 SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_names)
 {
   auto f = async_parameters_client_->get_parameters(parameter_names);
@@ -358,7 +367,7 @@ SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_
     return f.get();
   }
   // Return an empty vector if unsuccessful
-  return std::vector<rclcpp::parameter::ParameterVariant>();
+  return std::vector<rclcpp::Parameter>();
 }
 
 bool
@@ -370,7 +379,7 @@ SyncParametersClient::has_parameter(const std::string & parameter_name)
   return vars.names.size() > 0;
 }
 
-std::vector<rclcpp::parameter::ParameterType>
+std::vector<rclcpp::ParameterType>
 SyncParametersClient::get_parameter_types(const std::vector<std::string> & parameter_names)
 {
   auto f = async_parameters_client_->get_parameter_types(parameter_names);
@@ -381,17 +390,18 @@ SyncParametersClient::get_parameter_types(const std::vector<std::string> & param
   {
     return f.get();
   }
-  return std::vector<rclcpp::parameter::ParameterType>();
+  return std::vector<rclcpp::ParameterType>();
 }
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
 SyncParametersClient::set_parameters(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   auto f = async_parameters_client_->set_parameters(parameters);
 
+  auto node_base_interface = node_->get_node_base_interface();
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
+  if (spin_node_until_future_complete(*executor_, node_base_interface, f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
@@ -401,7 +411,7 @@ SyncParametersClient::set_parameters(
 
 rcl_interfaces::msg::SetParametersResult
 SyncParametersClient::set_parameters_atomically(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   auto f = async_parameters_client_->set_parameters_atomically(parameters);
 

rclcpp/src/rclcpp/parameter_map.cpp

@@ -0,0 +1,128 @@
+// 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 <string>
+#include <vector>
+
+#include "rclcpp/parameter_map.hpp"
+
+using rclcpp::exceptions::InvalidParametersException;
+using rclcpp::exceptions::InvalidParameterValueException;
+using rclcpp::ParameterMap;
+using rclcpp::ParameterValue;
+
+ParameterMap
+rclcpp::parameter_map_from(const rcl_params_t * const c_params)
+{
+  if (NULL == c_params) {
+    throw InvalidParametersException("parameters struct is NULL");
+  } else if (NULL == c_params->node_names) {
+    throw InvalidParametersException("node names array is NULL");
+  } else if (NULL == c_params->params) {
+    throw InvalidParametersException("node params array is NULL");
+  }
+
+  // Convert c structs into a list of parameters to set
+  ParameterMap parameters;
+  for (size_t n = 0; n < c_params->num_nodes; ++n) {
+    const char * c_node_name = c_params->node_names[n];
+    if (NULL == c_node_name) {
+      throw InvalidParametersException("Node name at index " + std::to_string(n) + " is NULL");
+    }
+
+    /// make sure there is a leading slash on the fully qualified node name
+    std::string node_name("/");
+    if ('/' != c_node_name[0]) {
+      node_name += c_node_name;
+    } else {
+      node_name = c_node_name;
+    }
+
+    const rcl_node_params_t * const c_params_node = &(c_params->params[n]);
+
+    std::vector<Parameter> & params_node = parameters[node_name];
+    params_node.reserve(c_params_node->num_params);
+
+    for (size_t p = 0; p < c_params_node->num_params; ++p) {
+      const char * const c_param_name = c_params_node->parameter_names[p];
+      if (NULL == c_param_name) {
+        std::string message(
+          "At node " + std::to_string(n) + " parameter " + std::to_string(p) + " name is NULL");
+        throw InvalidParametersException(message);
+      }
+      const rcl_variant_t * const c_param_value = &(c_params_node->parameter_values[p]);
+      params_node.emplace_back(c_param_name, parameter_value_from(c_param_value));
+    }
+  }
+  return parameters;
+}
+
+ParameterValue
+rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value)
+{
+  if (NULL == c_param_value) {
+    throw InvalidParameterValueException("Passed argument is NULL");
+  }
+  if (c_param_value->bool_value) {
+    return ParameterValue(*(c_param_value->bool_value));
+  } else if (c_param_value->integer_value) {
+    return ParameterValue(*(c_param_value->integer_value));
+  } else if (c_param_value->double_value) {
+    return ParameterValue(*(c_param_value->double_value));
+  } else if (c_param_value->string_value) {
+    return ParameterValue(std::string(c_param_value->string_value));
+  } else if (c_param_value->byte_array_value) {
+    const rcl_byte_array_t * const byte_array = c_param_value->byte_array_value;
+    std::vector<uint8_t> bytes;
+    bytes.reserve(byte_array->size);
+    for (size_t v = 0; v < byte_array->size; ++v) {
+      bytes.push_back(byte_array->values[v]);
+    }
+    return ParameterValue(bytes);
+  } else if (c_param_value->bool_array_value) {
+    const rcl_bool_array_t * const bool_array = c_param_value->bool_array_value;
+    std::vector<bool> bools;
+    bools.reserve(bool_array->size);
+    for (size_t v = 0; v < bool_array->size; ++v) {
+      bools.push_back(bool_array->values[v]);
+    }
+    return ParameterValue(bools);
+  } else if (c_param_value->integer_array_value) {
+    const rcl_int64_array_t * const int_array = c_param_value->integer_array_value;
+    std::vector<int64_t> integers;
+    integers.reserve(int_array->size);
+    for (size_t v = 0; v < int_array->size; ++v) {
+      integers.push_back(int_array->values[v]);
+    }
+    return ParameterValue(integers);
+  } else if (c_param_value->double_array_value) {
+    const rcl_double_array_t * const double_array = c_param_value->double_array_value;
+    std::vector<double> doubles;
+    doubles.reserve(double_array->size);
+    for (size_t v = 0; v < double_array->size; ++v) {
+      doubles.push_back(double_array->values[v]);
+    }
+    return ParameterValue(doubles);
+  } else if (c_param_value->string_array_value) {
+    const rcutils_string_array_t * const string_array = c_param_value->string_array_value;
+    std::vector<std::string> strings;
+    strings.reserve(string_array->size);
+    for (size_t v = 0; v < string_array->size; ++v) {
+      strings.emplace_back(string_array->data[v]);
+    }
+    return ParameterValue(strings);
+  }
+
+  throw InvalidParameterValueException("No parameter value set");
+}

rclcpp/src/rclcpp/parameter_service.cpp

@@ -24,120 +24,106 @@
 using rclcpp::ParameterService;
 
 ParameterService::ParameterService(
-  const rclcpp::Node::SharedPtr node,
+  const std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base,
+  const std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services,
+  rclcpp::node_interfaces::NodeParametersInterface * node_params,
   const rmw_qos_profile_t & qos_profile)
-: node_(node)
 {
-  std::weak_ptr<rclcpp::Node> captured_node = node_;
-  get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::get_parameters,
-    [captured_node](
+  const std::string node_name = node_base->get_name();
+
+  get_parameters_service_ = create_service<rcl_interfaces::srv::GetParameters>(
+    node_base, node_services,
+    node_name + "/" + parameter_service_names::get_parameters,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::GetParameters::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
-      auto values = node->get_parameters(request->names);
-      for (auto & pvariant : values) {
-        response->values.push_back(pvariant.get_parameter_value());
+      for (const auto & name : request->names) {
+        // Default construct param to NOT_SET
+        rclcpp::Parameter param;
+        node_params->get_parameter(name, param);
+        // push back NOT_SET when get_parameter() call fails
+        response->values.push_back(param.get_value_message());
       }
     },
-    qos_profile);
+    qos_profile, nullptr);
 
-  get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::get_parameter_types,
-    [captured_node](
+  get_parameter_types_service_ = create_service<rcl_interfaces::srv::GetParameterTypes>(
+    node_base, node_services,
+    node_name + "/" + parameter_service_names::get_parameter_types,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
       std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
-      auto types = node->get_parameter_types(request->names);
+      auto types = node_params->get_parameter_types(request->names);
       std::transform(types.cbegin(), types.cend(),
       std::back_inserter(response->types), [](const uint8_t & type) {
-        return static_cast<rclcpp::parameter::ParameterType>(type);
+        return static_cast<rclcpp::ParameterType>(type);
       });
     },
-    qos_profile);
+    qos_profile, nullptr);
 
-  set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters,
-    [captured_node](
+  set_parameters_service_ = create_service<rcl_interfaces::srv::SetParameters>(
+    node_base, node_services,
+    node_name + "/" + parameter_service_names::set_parameters,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::SetParameters::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
-      std::vector<rclcpp::parameter::ParameterVariant> pvariants;
+      std::vector<rclcpp::Parameter> pvariants;
       for (auto & p : request->parameters) {
-        pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p));
+        pvariants.push_back(rclcpp::Parameter::from_parameter_msg(p));
       }
-      auto results = node->set_parameters(pvariants);
+      auto results = node_params->set_parameters(pvariants);
       response->results = results;
     },
-    qos_profile);
+    qos_profile, nullptr);
 
-  set_parameters_atomically_service_ =
-    node_->create_service<rcl_interfaces::srv::SetParametersAtomically>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters_atomically,
-    [captured_node](
+  set_parameters_atomically_service_ = create_service<rcl_interfaces::srv::SetParametersAtomically>(
+    node_base, node_services,
+    node_name + "/" + parameter_service_names::set_parameters_atomically,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
       std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
-      std::vector<rclcpp::parameter::ParameterVariant> pvariants;
+      std::vector<rclcpp::Parameter> pvariants;
       std::transform(request->parameters.cbegin(), request->parameters.cend(),
       std::back_inserter(pvariants),
       [](const rcl_interfaces::msg::Parameter & p) {
-        return rclcpp::parameter::ParameterVariant::from_parameter(p);
+        return rclcpp::Parameter::from_parameter_msg(p);
       });
-      auto result = node->set_parameters_atomically(pvariants);
+      auto result = node_params->set_parameters_atomically(pvariants);
       response->result = result;
     },
-    qos_profile);
+    qos_profile, nullptr);
 
-  describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::describe_parameters,
-    [captured_node](
+  describe_parameters_service_ = create_service<rcl_interfaces::srv::DescribeParameters>(
+    node_base, node_services,
+    node_name + "/" + parameter_service_names::describe_parameters,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
-      auto descriptors = node->describe_parameters(request->names);
+      auto descriptors = node_params->describe_parameters(request->names);
       response->descriptors = descriptors;
     },
-    qos_profile);
+    qos_profile, nullptr);
 
-  list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
-    std::string(node_->get_name()) + "/" + parameter_service_names::list_parameters,
-    [captured_node](
+  list_parameters_service_ = create_service<rcl_interfaces::srv::ListParameters>(
+    node_base, node_services,
+    node_name + "/" + parameter_service_names::list_parameters,
+    [node_params](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,
       std::shared_ptr<rcl_interfaces::srv::ListParameters::Response> response)
     {
-      auto node = captured_node.lock();
-      if (!node) {
-        return;
-      }
-      auto result = node->list_parameters(request->prefixes, request->depth);
+      auto result = node_params->list_parameters(request->prefixes, request->depth);
       response->result = result;
     },
-    qos_profile);
+    qos_profile, nullptr);
 }

rclcpp/src/rclcpp/parameter_value.cpp

@@ -0,0 +1,229 @@
+// 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 "rclcpp/parameter_value.hpp"
+
+#include <string>
+#include <vector>
+
+using rclcpp::ParameterType;
+using rclcpp::ParameterValue;
+
+std::string
+rclcpp::to_string(const ParameterType type)
+{
+  switch (type) {
+    case ParameterType::PARAMETER_NOT_SET:
+      return "not set";
+    case ParameterType::PARAMETER_BOOL:
+      return "bool";
+    case ParameterType::PARAMETER_INTEGER:
+      return "integer";
+    case ParameterType::PARAMETER_DOUBLE:
+      return "double";
+    case ParameterType::PARAMETER_STRING:
+      return "string";
+    case ParameterType::PARAMETER_BYTE_ARRAY:
+      return "byte_array";
+    case ParameterType::PARAMETER_BOOL_ARRAY:
+      return "bool_array";
+    case ParameterType::PARAMETER_INTEGER_ARRAY:
+      return "integer_array";
+    case ParameterType::PARAMETER_DOUBLE_ARRAY:
+      return "double_array";
+    case ParameterType::PARAMETER_STRING_ARRAY:
+      return "string_array";
+    default:
+      return "unknown type";
+  }
+}
+
+std::ostream &
+rclcpp::operator<<(std::ostream & os, const ParameterType type)
+{
+  os << rclcpp::to_string(type);
+  return os;
+}
+
+template<typename ValType, typename PrintType = ValType>
+std::string
+array_to_string(
+  const std::vector<ValType> & array,
+  const std::ios::fmtflags format_flags = std::ios::dec)
+{
+  std::stringstream type_array;
+  bool first_item = true;
+  type_array << "[";
+  type_array.setf(format_flags, std::ios_base::basefield | std::ios::boolalpha);
+  type_array << std::showbase;
+  for (const ValType value : array) {
+    if (!first_item) {
+      type_array << ", ";
+    } else {
+      first_item = false;
+    }
+    type_array << static_cast<PrintType>(value);
+  }
+  type_array << "]";
+  return type_array.str();
+}
+
+std::string
+rclcpp::to_string(const ParameterValue & value)
+{
+  switch (value.get_type()) {
+    case ParameterType::PARAMETER_NOT_SET:
+      return "not set";
+    case ParameterType::PARAMETER_BOOL:
+      return value.get<bool>() ? "true" : "false";
+    case ParameterType::PARAMETER_INTEGER:
+      return std::to_string(value.get<int>());
+    case ParameterType::PARAMETER_DOUBLE:
+      return std::to_string(value.get<double>());
+    case ParameterType::PARAMETER_STRING:
+      return value.get<std::string>();
+    case ParameterType::PARAMETER_BYTE_ARRAY:
+      return array_to_string<uint8_t, int>(value.get<std::vector<uint8_t>>(), std::ios::hex);
+    case ParameterType::PARAMETER_BOOL_ARRAY:
+      return array_to_string(value.get<std::vector<bool>>(), std::ios::boolalpha);
+    case ParameterType::PARAMETER_INTEGER_ARRAY:
+      return array_to_string(value.get<std::vector<int64_t>>());
+    case ParameterType::PARAMETER_DOUBLE_ARRAY:
+      return array_to_string(value.get<std::vector<double>>());
+    case ParameterType::PARAMETER_STRING_ARRAY:
+      return array_to_string(value.get<std::vector<std::string>>());
+    default:
+      return "unknown type";
+  }
+}
+
+ParameterValue::ParameterValue()
+{
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET;
+}
+
+ParameterValue::ParameterValue(const rcl_interfaces::msg::ParameterValue & value)
+{
+  value_ = value;
+  switch (value.type) {
+    case PARAMETER_BOOL:
+    case PARAMETER_INTEGER:
+    case PARAMETER_DOUBLE:
+    case PARAMETER_STRING:
+    case PARAMETER_BYTE_ARRAY:
+    case PARAMETER_BOOL_ARRAY:
+    case PARAMETER_INTEGER_ARRAY:
+    case PARAMETER_DOUBLE_ARRAY:
+    case PARAMETER_STRING_ARRAY:
+    case PARAMETER_NOT_SET:
+      break;
+    default:
+      // TODO(wjwwood): use custom exception
+      throw std::runtime_error("Unknown type: " + std::to_string(value.type));
+  }
+}
+
+ParameterValue::ParameterValue(const bool bool_value)
+{
+  value_.bool_value = bool_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+}
+
+ParameterValue::ParameterValue(const int int_value)
+{
+  value_.integer_value = int_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+}
+
+ParameterValue::ParameterValue(const int64_t int_value)
+{
+  value_.integer_value = int_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
+}
+
+ParameterValue::ParameterValue(const float double_value)
+{
+  value_.double_value = double_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+}
+
+ParameterValue::ParameterValue(const double double_value)
+{
+  value_.double_value = double_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+}
+
+ParameterValue::ParameterValue(const std::string & string_value)
+{
+  value_.string_value = string_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
+}
+
+ParameterValue::ParameterValue(const char * string_value)
+: ParameterValue(std::string(string_value))
+{}
+
+ParameterValue::ParameterValue(const std::vector<uint8_t> & byte_array_value)
+{
+  value_.byte_array_value = byte_array_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY;
+}
+
+ParameterValue::ParameterValue(const std::vector<bool> & bool_array_value)
+{
+  value_.bool_array_value = bool_array_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY;
+}
+
+ParameterValue::ParameterValue(const std::vector<int> & int_array_value)
+{
+  value_.integer_array_value.assign(int_array_value.cbegin(), int_array_value.cend());
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
+}
+
+ParameterValue::ParameterValue(const std::vector<int64_t> & int_array_value)
+{
+  value_.integer_array_value = int_array_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
+}
+
+ParameterValue::ParameterValue(const std::vector<float> & float_array_value)
+{
+  value_.double_array_value.assign(float_array_value.cbegin(), float_array_value.cend());
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY;
+}
+
+ParameterValue::ParameterValue(const std::vector<double> & double_array_value)
+{
+  value_.double_array_value = double_array_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY;
+}
+
+ParameterValue::ParameterValue(const std::vector<std::string> & string_array_value)
+{
+  value_.string_array_value = string_array_value;
+  value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY;
+}
+
+ParameterType
+ParameterValue::get_type() const
+{
+  return static_cast<ParameterType>(value_.type);
+}
+
+rcl_interfaces::msg::ParameterValue
+ParameterValue::to_value_msg() const
+{
+  return value_;
+}

rclcpp/src/rclcpp/publisher.cpp

@@ -24,6 +24,7 @@
 #include <string>
 
 #include "rcl_interfaces/msg/intra_process_message.hpp"
+#include "rcutils/logging_macros.h"
 #include "rmw/impl/cpp/demangle.hpp"
 
 #include "rclcpp/allocator/allocator_common.hpp"
@@ -79,17 +80,17 @@ PublisherBase::PublisherBase(
 PublisherBase::~PublisherBase()
 {
   if (rcl_publisher_fini(&intra_process_publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
-    fprintf(
-      stderr,
-      "Error in destruction of intra process rcl publisher handle: %s\n",
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "Error in destruction of intra process rcl publisher handle: %s",
       rcl_get_error_string_safe());
     rcl_reset_error();
   }
 
   if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
-    fprintf(
-      stderr,
-      "Error in destruction of rcl publisher handle: %s\n",
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "Error in destruction of rcl publisher handle: %s",
       rcl_get_error_string_safe());
     rcl_reset_error();
   }
@@ -170,7 +171,13 @@ PublisherBase::setup_intra_process(
   StoreMessageCallbackT callback,
   const rcl_publisher_options_t & intra_process_options)
 {
-  auto intra_process_topic_name = std::string(this->get_topic_name()) + "/_intra";
+  const char * topic_name = this->get_topic_name();
+  if (!topic_name) {
+    throw std::runtime_error("failed to get topic name");
+  }
+
+  auto intra_process_topic_name = std::string(topic_name) + "/_intra";
+
   rcl_ret_t ret = rcl_publisher_init(
     &intra_process_publisher_handle_,
     rcl_node_handle_.get(),

rclcpp/src/rclcpp/service.cpp

@@ -27,12 +27,6 @@
 
 using rclcpp::ServiceBase;
 
-ServiceBase::ServiceBase(
-  std::shared_ptr<rcl_node_t> node_handle,
-  const std::string & service_name)
-: node_handle_(node_handle), service_name_(service_name)
-{}
-
 ServiceBase::ServiceBase(std::shared_ptr<rcl_node_t> node_handle)
 : node_handle_(node_handle)
 {}
@@ -40,19 +34,19 @@ ServiceBase::ServiceBase(std::shared_ptr<rcl_node_t> node_handle)
 ServiceBase::~ServiceBase()
 {}
 
-std::string
+const char *
 ServiceBase::get_service_name()
 {
-  return this->service_name_;
+  return rcl_service_get_service_name(this->get_service_handle().get());
 }
 
-rcl_service_t *
+std::shared_ptr<rcl_service_t>
 ServiceBase::get_service_handle()
 {
   return service_handle_;
 }
 
-const rcl_service_t *
+std::shared_ptr<const rcl_service_t>
 ServiceBase::get_service_handle() const
 {
   return service_handle_;

rclcpp/src/rclcpp/subscription.cpp

@@ -20,22 +20,54 @@
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
+#include "rclcpp/logging.hpp"
 
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
-
 using rclcpp::SubscriptionBase;
 
 SubscriptionBase::SubscriptionBase(
   std::shared_ptr<rcl_node_t> node_handle,
   const rosidl_message_type_support_t & type_support_handle,
   const std::string & topic_name,
-  const rcl_subscription_options_t & subscription_options)
-: node_handle_(node_handle)
+  const rcl_subscription_options_t & subscription_options,
+  bool is_serialized)
+: node_handle_(node_handle),
+  type_support_(type_support_handle),
+  is_serialized_(is_serialized)
 {
+  std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
+  auto custom_deletor = [weak_node_handle](rcl_subscription_t * rcl_subs)
+    {
+      auto handle = weak_node_handle.lock();
+      if (handle) {
+        if (rcl_subscription_fini(rcl_subs, handle.get()) != RCL_RET_OK) {
+          RCLCPP_ERROR(
+            rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
+            "Error in destruction of rcl subscription handle: %s",
+            rcl_get_error_string_safe());
+          rcl_reset_error();
+        }
+      } else {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"),
+          "Error in destruction of rcl subscription handle: "
+          "the Node Handle was destructed too early. You will leak memory");
+      }
+      delete rcl_subs;
+    };
+
+  subscription_handle_ = std::shared_ptr<rcl_subscription_t>(
+    new rcl_subscription_t, custom_deletor);
+  *subscription_handle_.get() = rcl_get_zero_initialized_subscription();
+
+  intra_process_subscription_handle_ = std::shared_ptr<rcl_subscription_t>(
+    new rcl_subscription_t, custom_deletor);
+  *intra_process_subscription_handle_.get() = rcl_get_zero_initialized_subscription();
+
   rcl_ret_t ret = rcl_subscription_init(
-    &subscription_handle_,
+    subscription_handle_.get(),
     node_handle_.get(),
     &type_support_handle,
     topic_name.c_str(),
@@ -57,42 +89,40 @@ SubscriptionBase::SubscriptionBase(
 
 SubscriptionBase::~SubscriptionBase()
 {
-  if (rcl_subscription_fini(&subscription_handle_, node_handle_.get()) != RCL_RET_OK) {
-    std::stringstream ss;
-    ss << "Error in destruction of rcl subscription handle: " <<
-      rcl_get_error_string_safe() << '\n';
-    (std::cerr << ss.str()).flush();
-  }
-  if (rcl_subscription_fini(
-      &intra_process_subscription_handle_, node_handle_.get()) != RCL_RET_OK)
-  {
-    std::stringstream ss;
-    ss << "Error in destruction of rmw intra process subscription handle: " <<
-      rcl_get_error_string_safe() << '\n';
-    (std::cerr << ss.str()).flush();
-  }
 }
 
 const char *
 SubscriptionBase::get_topic_name() const
 {
-  return rcl_subscription_get_topic_name(&subscription_handle_);
+  return rcl_subscription_get_topic_name(subscription_handle_.get());
 }
 
-rcl_subscription_t *
+std::shared_ptr<rcl_subscription_t>
 SubscriptionBase::get_subscription_handle()
 {
-  return &subscription_handle_;
+  return subscription_handle_;
 }
 
-const rcl_subscription_t *
+const std::shared_ptr<rcl_subscription_t>
 SubscriptionBase::get_subscription_handle() const
 {
-  return &subscription_handle_;
+  return subscription_handle_;
 }
 
-const rcl_subscription_t *
+const std::shared_ptr<rcl_subscription_t>
 SubscriptionBase::get_intra_process_subscription_handle() const
 {
-  return &intra_process_subscription_handle_;
+  return intra_process_subscription_handle_;
+}
+
+const rosidl_message_type_support_t &
+SubscriptionBase::get_message_type_support_handle() const
+{
+  return type_support_;
+}
+
+bool
+SubscriptionBase::is_serialized() const
+{
+  return is_serialized_;
 }

rclcpp/src/rclcpp/time.cpp

@@ -26,6 +26,8 @@
 
 #include "rcutils/logging_macros.h"
 
+#include "rclcpp/utilities.hpp"
+
 namespace
 {
 
@@ -50,11 +52,11 @@ Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type)
     throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
   }
 
-  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(seconds));
+  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(seconds));
   rcl_time_.nanoseconds += nanoseconds;
 }
 
-Time::Time(uint64_t nanoseconds, rcl_clock_type_t clock_type)
+Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type)
 : rcl_time_(init_time_point(clock_type))
 {
   rcl_time_.nanoseconds = nanoseconds;
@@ -75,7 +77,7 @@ Time::Time(
     throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
   }
 
-  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(time_msg.sec));
+  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(time_msg.sec));
   rcl_time_.nanoseconds += time_msg.nanosec;
 }
 
@@ -115,7 +117,7 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg)
   rcl_clock_type_t ros_time = RCL_ROS_TIME;
   rcl_time_ = init_time_point(ros_time);  // TODO(tfoote) hard coded ROS here
 
-  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(time_msg.sec));
+  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(time_msg.sec));
   rcl_time_.nanoseconds += time_msg.nanosec;
   return *this;
 }
@@ -179,11 +181,11 @@ Time::operator>(const rclcpp::Time & rhs) const
 Time
 Time::operator+(const rclcpp::Duration & rhs) const
 {
-  if (rhs.nanoseconds() > 0 && (uint64_t)rhs.nanoseconds() >
-    std::numeric_limits<rcl_time_point_value_t>::max() -
-    (rcl_time_point_value_t)this->nanoseconds())
-  {
-    throw std::overflow_error("addition leads to uint64_t overflow");
+  if (rclcpp::add_will_overflow(rhs.nanoseconds(), this->nanoseconds())) {
+    throw std::overflow_error("addition leads to int64_t overflow");
+  }
+  if (rclcpp::add_will_underflow(rhs.nanoseconds(), this->nanoseconds())) {
+    throw std::underflow_error("addition leads to int64_t underflow");
   }
   return Time(this->nanoseconds() + rhs.nanoseconds(), this->get_clock_type());
 }
@@ -195,17 +197,12 @@ Time::operator-(const rclcpp::Time & rhs) const
     throw std::runtime_error("can't subtract times with different time sources");
   }
 
-  if (rcl_time_.nanoseconds >
-    (uint64_t)std::numeric_limits<rcl_duration_value_t>::max() + rhs.rcl_time_.nanoseconds)
-  {
-    throw std::underflow_error("time subtraction leads to int64_t overflow");
+  if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.rcl_time_.nanoseconds)) {
+    throw std::overflow_error("time subtraction leads to int64_t overflow");
   }
 
-  if (rcl_time_.nanoseconds < rhs.rcl_time_.nanoseconds) {
-    rcl_time_point_value_t negative_delta = rhs.rcl_time_.nanoseconds - rcl_time_.nanoseconds;
-    if (negative_delta > (uint64_t) std::numeric_limits<rcl_duration_value_t>::min()) {
-      throw std::underflow_error("time subtraction leads to int64_t underflow");
-    }
+  if (rclcpp::sub_will_underflow(rcl_time_.nanoseconds, rhs.rcl_time_.nanoseconds)) {
+    throw std::underflow_error("time subtraction leads to int64_t underflow");
   }
 
   return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
@@ -214,21 +211,17 @@ Time::operator-(const rclcpp::Time & rhs) const
 Time
 Time::operator-(const rclcpp::Duration & rhs) const
 {
-  if (rhs.nanoseconds() > 0 && rcl_time_.nanoseconds >
-    std::numeric_limits<rcl_time_point_value_t>::max() - (uint64_t)rhs.nanoseconds())
-  {
-    throw std::underflow_error("time subtraction leads to uint64_t overflow");
+  if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.nanoseconds())) {
+    throw std::overflow_error("time subtraction leads to int64_t overflow");
   }
-  if (rcl_time_.nanoseconds < (uint64_t) std::numeric_limits<rcl_duration_value_t>::max() &&
-    (int64_t)rcl_time_.nanoseconds < (int64_t)rhs.nanoseconds())
-  {
-    throw std::underflow_error("time subtraction leads to uint64_t underflow");
+  if (rclcpp::sub_will_underflow(rcl_time_.nanoseconds, rhs.nanoseconds())) {
+    throw std::underflow_error("time subtraction leads to int64_t underflow");
   }
 
   return Time(rcl_time_.nanoseconds - rhs.nanoseconds(), rcl_time_.clock_type);
 }
 
-uint64_t
+int64_t
 Time::nanoseconds() const
 {
   return rcl_time_.nanoseconds;
@@ -243,10 +236,11 @@ Time::get_clock_type() const
 Time
 operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs)
 {
-  if (rhs.nanoseconds() >
-    std::numeric_limits<rcl_time_point_value_t>::max() - (rcl_time_point_value_t)lhs.nanoseconds())
-  {
-    throw std::overflow_error("addition leads to uint64_t overflow");
+  if (rclcpp::add_will_overflow(rhs.nanoseconds(), lhs.nanoseconds())) {
+    throw std::overflow_error("addition leads to int64_t overflow");
+  }
+  if (rclcpp::add_will_underflow(rhs.nanoseconds(), lhs.nanoseconds())) {
+    throw std::underflow_error("addition leads to int64_t underflow");
   }
   return Time(lhs.nanoseconds() + rhs.nanoseconds(), rhs.get_clock_type());
 }

rclcpp/src/rclcpp/time_source.cpp

@@ -74,7 +74,9 @@ void TimeSource::attachNode(
 
   auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1);
 
-  clock_subscription_ = rclcpp::create_subscription<MessageT, decltype(cb), Alloc, SubscriptionT>(
+  clock_subscription_ = rclcpp::create_subscription<
+    MessageT, decltype(cb), Alloc, MessageT, SubscriptionT
+    >(
     node_topics_.get(),
     topic_name,
     std::move(cb),
@@ -118,7 +120,8 @@ void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
   associated_clocks_.push_back(clock);
   // Set the clock if there's already data for it
   if (last_msg_set_) {
-    set_clock(last_msg_set_, ros_time_active_, clock);
+    auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
+    set_clock(time_msg, ros_time_active_, clock);
   }
 }
 
@@ -194,17 +197,18 @@ void TimeSource::set_clock(
   clock->invoke_postjump_callbacks(active_callbacks, jump);
 }
 
-void TimeSource::clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg)
+void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg)
 {
   if (!this->ros_time_active_) {
     enable_ros_time();
   }
   // Cache the last message in case a new clock is attached.
   last_msg_set_ = msg;
+  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
 
   std::lock_guard<std::mutex> guard(clock_list_lock_);
   for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
-    set_clock(msg, true, *it);
+    set_clock(time_msg, true, *it);
   }
 }
 
@@ -215,7 +219,7 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
     {rclcpp::ParameterEventsFilter::EventType::NEW,
       rclcpp::ParameterEventsFilter::EventType::CHANGED});
   for (auto & it : filter.get_events()) {
-    if (it.second->value.type != parameter::ParameterType::PARAMETER_BOOL) {
+    if (it.second->value.type != ParameterType::PARAMETER_BOOL) {
       RCUTILS_LOG_ERROR("use_sim_time parameter set to something besides a bool");
       continue;
     }

rclcpp/src/rclcpp/timer.cpp

@@ -16,16 +16,36 @@
 
 #include <chrono>
 #include <string>
+#include <memory>
+
+#include "rcutils/logging_macros.h"
 
 using rclcpp::TimerBase;
 
 TimerBase::TimerBase(std::chrono::nanoseconds period)
 {
+  timer_handle_ = std::shared_ptr<rcl_timer_t>(
+    new rcl_timer_t, [ = ](rcl_timer_t * timer)
+    {
+      if (rcl_timer_fini(timer) != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Failed to clean up rcl timer handle: %s", rcl_get_error_string_safe());
+        rcl_reset_error();
+      }
+      delete timer;
+    });
+
+  *timer_handle_.get() = rcl_get_zero_initialized_timer();
+
   if (rcl_timer_init(
-      &timer_handle_, period.count(), nullptr,
+      timer_handle_.get(), period.count(), nullptr,
       rcl_get_default_allocator()) != RCL_RET_OK)
   {
-    fprintf(stderr, "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe());
+    RCUTILS_LOG_ERROR_NAMED(
+      "rclcpp",
+      "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe());
+    rcl_reset_error();
   }
 }
 
@@ -35,7 +55,7 @@ TimerBase::~TimerBase()
 void
 TimerBase::cancel()
 {
-  if (rcl_timer_cancel(&timer_handle_) != RCL_RET_OK) {
+  if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) {
     throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string_safe());
   }
 }
@@ -43,7 +63,7 @@ TimerBase::cancel()
 void
 TimerBase::reset()
 {
-  if (rcl_timer_reset(&timer_handle_) != RCL_RET_OK) {
+  if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) {
     throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string_safe());
   }
 }
@@ -52,7 +72,7 @@ bool
 TimerBase::is_ready()
 {
   bool ready = false;
-  if (rcl_timer_is_ready(&timer_handle_, &ready) != RCL_RET_OK) {
+  if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) {
     throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string_safe());
   }
   return ready;
@@ -62,7 +82,9 @@ std::chrono::nanoseconds
 TimerBase::time_until_trigger()
 {
   int64_t time_until_next_call = 0;
-  if (rcl_timer_get_time_until_next_call(&timer_handle_, &time_until_next_call) != RCL_RET_OK) {
+  if (rcl_timer_get_time_until_next_call(timer_handle_.get(),
+    &time_until_next_call) != RCL_RET_OK)
+  {
     throw std::runtime_error(
             std::string("Timer could not get time until next call: ") +
             rcl_get_error_string_safe());
@@ -70,8 +92,8 @@ TimerBase::time_until_trigger()
   return std::chrono::nanoseconds(time_until_next_call);
 }
 
-const rcl_timer_t *
+std::shared_ptr<const rcl_timer_t>
 TimerBase::get_timer_handle()
 {
-  return &timer_handle_;
+  return timer_handle_;
 }

rclcpp/src/rclcpp/utilities.cpp

@@ -24,12 +24,16 @@
 #include <string>
 #include <vector>
 
+#include "rclcpp/exceptions.hpp"
+
 #include "rcl/error_handling.h"
 #include "rcl/rcl.h"
 
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
+#include "rcutils/logging_macros.h"
+
 // Determine if sigaction is available
 #if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
 #define HAS_SIGACTION
@@ -114,8 +118,9 @@ trigger_interrupt_guard_condition(int signal_value)
     for (auto & kv : g_sigint_guard_cond_handles) {
       rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second));
       if (status != RCL_RET_OK) {
-        fprintf(stderr,
-          "[rclcpp::error] failed to trigger guard condition: %s\n", rcl_get_error_string_safe());
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "failed to trigger guard condition: %s", rcl_get_error_string_safe());
       }
     }
   }
@@ -157,7 +162,7 @@ signal_handler(int signal_value)
 }
 
 void
-rclcpp::init(int argc, char * argv[])
+rclcpp::init(int argc, char const * const argv[])
 {
   g_is_interrupted.store(false);
   if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
@@ -188,6 +193,71 @@ rclcpp::init(int argc, char * argv[])
 #endif
 }
 
+std::vector<std::string>
+rclcpp::init_and_remove_ros_arguments(int argc, char const * const argv[])
+{
+  rclcpp::init(argc, argv);
+  return rclcpp::remove_ros_arguments(argc, argv);
+}
+
+std::vector<std::string>
+rclcpp::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();
+
+  rcl_ret_t ret;
+
+  ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
+  if (RCL_RET_OK != ret) {
+    exceptions::throw_from_rcl_error(ret, "failed to parse arguments");
+  }
+
+  int nonros_argc = 0;
+  const char ** nonros_argv = NULL;
+
+  ret = rcl_remove_ros_arguments(
+    argv,
+    &parsed_args,
+    alloc,
+    &nonros_argc,
+    &nonros_argv);
+
+  if (RCL_RET_OK != ret) {
+    // Not using throw_from_rcl_error, because we may need to append deallocation failures.
+    exceptions::RCLErrorBase base_exc(ret, rcl_get_error_state());
+    rcl_reset_error();
+    if (NULL != nonros_argv) {
+      alloc.deallocate(nonros_argv, alloc.state);
+    }
+    if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) {
+      base_exc.formatted_message += std::string(
+        ", failed also to cleanup parsed arguments, leaking memory: ") +
+        rcl_get_error_string_safe();
+      rcl_reset_error();
+    }
+    throw exceptions::RCLError(base_exc, "");
+  }
+
+  std::vector<std::string> return_arguments;
+  return_arguments.resize(nonros_argc);
+
+  for (int ii = 0; ii < nonros_argc; ++ii) {
+    return_arguments[ii] = std::string(nonros_argv[ii]);
+  }
+
+  if (NULL != nonros_argv) {
+    alloc.deallocate(nonros_argv, alloc.state);
+  }
+
+  ret = rcl_arguments_fini(&parsed_args);
+  if (RCL_RET_OK != ret) {
+    exceptions::throw_from_rcl_error(ret, "failed to cleanup parsed arguments, leaking memory");
+  }
+
+  return return_arguments;
+}
+
 bool
 rclcpp::ok()
 {

rclcpp/test/executors/test_multi_threaded_executor.cpp

@@ -0,0 +1,103 @@
+// 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 <chrono>
+#include <string>
+#include <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/executors.hpp"
+
+#include "rcl_interfaces/msg/intra_process_message.hpp"
+
+using namespace std::chrono_literals;
+
+using rcl_interfaces::msg::IntraProcessMessage;
+
+class TestMultiThreadedExecutor : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+/*
+   Test that timers are not taken multiple times when using reentrant callback groups.
+ */
+TEST_F(TestMultiThreadedExecutor, timer_over_take) {
+#ifdef __linux__
+  // This seems to be the most effective way to force the bug to happen on Linux.
+  // This is unnecessary on MacOS, since the default scheduler causes it.
+  struct sched_param param;
+  param.sched_priority = 0;
+  if (sched_setscheduler(0, SCHED_BATCH, &param) != 0) {
+    perror("sched_setscheduler");
+  }
+#endif
+
+  bool yield_before_execute = true;
+
+  rclcpp::executors::MultiThreadedExecutor executor(
+    rclcpp::executor::create_default_executor_arguments(), 2u, yield_before_execute);
+
+  ASSERT_GT(executor.get_number_of_threads(), 1u);
+
+  std::shared_ptr<rclcpp::Node> node =
+    std::make_shared<rclcpp::Node>("test_multi_threaded_executor_timer_over_take");
+
+  auto cbg = node->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant);
+
+  rclcpp::Clock system_clock(RCL_STEADY_TIME);
+  std::mutex last_mutex;
+  auto last = system_clock.now();
+
+  std::atomic_int timer_count {0};
+
+  auto timer_callback = [&timer_count, &executor, &system_clock, &last_mutex, &last]() {
+      // While this tolerance is a little wide, if the bug occurs, the next step will
+      // happen almost instantly. The purpose of this test is not to measure the jitter
+      // in timers, just assert that a reasonable amount of time has passed.
+      const double PERIOD = 0.1f;
+      const double TOLERANCE = 0.025f;
+
+      rclcpp::Time now = system_clock.now();
+      timer_count++;
+
+      if (timer_count > 5) {
+        executor.cancel();
+      }
+
+      {
+        std::lock_guard<std::mutex> lock(last_mutex);
+        double diff = std::abs((now - last).nanoseconds()) / 1.0e9;
+        last = now;
+
+        if (diff < PERIOD - TOLERANCE || diff > PERIOD + TOLERANCE) {
+          executor.cancel();
+          ASSERT_GT(diff, PERIOD - TOLERANCE);
+          ASSERT_LT(diff, PERIOD + TOLERANCE);
+        }
+      }
+    };
+
+  auto timer = node->create_wall_timer(100ms, timer_callback, cbg);
+  executor.add_node(node);
+  executor.spin();
+}

rclcpp/test/test_duration.cpp

@@ -54,6 +54,9 @@ TEST(TestDuration, operators) {
   EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds()));
   EXPECT_EQ(sub, young - old);
 
+  rclcpp::Duration scale = old * 3;
+  EXPECT_EQ(scale.nanoseconds(), (rcl_duration_value_t)(old.nanoseconds() * 3));
+
   rclcpp::Duration time = rclcpp::Duration(0, 0);
   rclcpp::Duration copy_constructor_duration(time);
   rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0);
@@ -85,4 +88,12 @@ TEST(TestDuration, overflows) {
   EXPECT_THROW(min - one, std::underflow_error);
   EXPECT_THROW(negative_one + min, std::underflow_error);
   EXPECT_THROW(negative_one - max, std::underflow_error);
+
+  rclcpp::Duration base_d = max * 0.3;
+  EXPECT_THROW(base_d * 4, std::overflow_error);
+  EXPECT_THROW(base_d * (-4), std::underflow_error);
+
+  rclcpp::Duration base_d_neg = max * (-0.3);
+  EXPECT_THROW(base_d_neg * (-4), std::overflow_error);
+  EXPECT_THROW(base_d_neg * 4, std::underflow_error);
 }

rclcpp/test/test_externally_defined_services.cpp

@@ -106,6 +106,15 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) {
     return;
   }
 
+  // Destruct the service
+  ret = rcl_service_fini(
+    &service_handle,
+    node_handle->get_node_base_interface()->get_rcl_node_handle());
+  if (ret != RCL_RET_OK) {
+    FAIL();
+    return;
+  }
+
   SUCCEED();
 }
 
@@ -139,5 +148,15 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) {
     FAIL();
     return;
   }
+
+  // Destruct the service
+  ret = rcl_service_fini(
+    &service_handle,
+    node_handle->get_node_base_interface()->get_rcl_node_handle());
+  if (ret != RCL_RET_OK) {
+    FAIL();
+    return;
+  }
+
   SUCCEED();
 }

rclcpp/test/test_find_weak_nodes.cpp

@@ -52,6 +52,9 @@ TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) {
   // THEN
   // The result of finding dangling node pointers should be true
   ASSERT_TRUE(has_invalid_weak_nodes);
+
+  // Prevent memory leak due to the order of destruction
+  memory_strategy->clear_handles();
 }
 
 TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) {
@@ -73,4 +76,7 @@ TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) {
   // THEN
   // The result of finding dangling node pointers should be false
   ASSERT_FALSE(has_invalid_weak_nodes);
+
+  // Prevent memory leak due to the order of destruction
+  memory_strategy->clear_handles();
 }

rclcpp/test/test_function_traits.cpp

@@ -706,3 +706,23 @@ TEST(TestFunctionTraits, sfinae_match) {
 
   EXPECT_EQ("foo", func_accept_callback_return_type(lambda_no_args_string));
 }
+
+class TestMember : public ::testing::Test
+{
+public:
+  void MemberFunctor(int, float, std::string) {}
+};
+
+/*
+   Regression test for https://github.com/ros2/rclcpp/issues/479, specific to classes using the
+   TEST_F GTest macro.
+*/
+TEST_F(TestMember, bind_member_functor) {
+  auto bind_member_functor = std::bind(&TestMember::MemberFunctor, this, std::placeholders::_1,
+      std::placeholders::_2, std::placeholders::_3);
+
+  static_assert(
+    rclcpp::function_traits::check_arguments<decltype(bind_member_functor), int, float,
+    std::string>::value,
+    "Functor accepts an int, a float and a string as arguments");
+}

rclcpp/test/test_logging.cpp

@@ -31,9 +31,10 @@ rclcpp::Logger g_logger = rclcpp::get_logger("name");
 
 struct LogEvent
 {
-  rcutils_log_location_t * location;
+  const rcutils_log_location_t * location;
   int level;
   std::string name;
+  rcutils_time_point_value_t timestamp;
   std::string message;
 };
 LogEvent g_last_log_event;
@@ -49,13 +50,15 @@ public:
     rcutils_logging_set_default_logger_level(RCUTILS_LOG_SEVERITY_DEBUG);
 
     auto rcutils_logging_console_output_handler = [](
-      rcutils_log_location_t * location,
-      int level, const char * name, const char * format, va_list * args) -> void
+      const rcutils_log_location_t * location,
+      int level, const char * name, rcutils_time_point_value_t timestamp,
+      const char * format, va_list * args) -> void
       {
         g_log_calls += 1;
         g_last_log_event.location = location;
         g_last_log_event.level = level;
         g_last_log_event.name = name ? name : "";
+        g_last_log_event.timestamp = timestamp;
         char buffer[1024];
         vsnprintf(buffer, sizeof(buffer), format, *args);
         g_last_log_event.message = buffer;
@@ -68,7 +71,7 @@ public:
   void TearDown()
   {
     rcutils_logging_set_output_handler(this->previous_output_handler);
-    g_rcutils_logging_initialized = false;
+    ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_shutdown());
     EXPECT_FALSE(g_rcutils_logging_initialized);
   }
 };
@@ -82,7 +85,7 @@ TEST_F(TestLoggingMacros, test_logging_named) {
   if (g_last_log_event.location) {
     EXPECT_STREQ("TestBody", g_last_log_event.location->function_name);
     EXPECT_THAT(g_last_log_event.location->file_name, EndsWith("test_logging.cpp"));
-    EXPECT_EQ(78u, g_last_log_event.location->line_number);
+    EXPECT_EQ(81u, g_last_log_event.location->line_number);
   }
   EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, g_last_log_event.level);
   EXPECT_EQ("name", g_last_log_event.name);

rclcpp/test/test_node.cpp

@@ -16,6 +16,7 @@
 
 #include <string>
 #include <memory>
+#include <vector>
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/node.hpp"
@@ -75,14 +76,25 @@ TEST_F(TestNode, get_name_and_namespace) {
 }
 
 TEST_F(TestNode, get_logger) {
-  // Currently the namespace is not taken into account with the node logger name
   {
     auto node = std::make_shared<rclcpp::Node>("my_node");
     EXPECT_STREQ("my_node", node->get_logger().get_name());
   }
   {
     auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
-    EXPECT_STREQ("my_node", node->get_logger().get_name());
+    EXPECT_STREQ("ns.my_node", node->get_logger().get_name());
+  }
+  {
+    auto node = std::make_shared<rclcpp::Node>("my_node", "ns");
+    EXPECT_STREQ("ns.my_node", node->get_logger().get_name());
+  }
+  {
+    auto node = std::make_shared<rclcpp::Node>("my_node", "/my/ns");
+    EXPECT_STREQ("my.ns.my_node", node->get_logger().get_name());
+  }
+  {
+    auto node = std::make_shared<rclcpp::Node>("my_node", "my/ns");
+    EXPECT_STREQ("my.ns.my_node", node->get_logger().get_name());
   }
 }
 
@@ -99,5 +111,5 @@ TEST_F(TestNode, now) {
   auto now_builtin = node->now().nanoseconds();
   auto now_external = clock->now().nanoseconds();
   EXPECT_GE(now_external, now_builtin);
-  EXPECT_LT(now_external - now_builtin, 50000ul);
+  EXPECT_LT(now_external - now_builtin, 50000L);
 }

rclcpp/test/test_node_global_args.cpp

@@ -0,0 +1,64 @@
+// 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/exceptions.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class TestNodeWithGlobalArgs : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    const char * const args[] = {"proc", "__node:=global_node_name"};
+    rclcpp::init(2, args);
+  }
+};
+
+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);
+  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);
+    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);
+    EXPECT_STREQ("global_node_name", node->get_name());
+  }
+}

rclcpp/test/test_node_initial_parameters.cpp

@@ -0,0 +1,70 @@
+// 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);
+  EXPECT_EQ(0u, list_params_result.names.size());
+}
+
+TEST_F(TestNodeWithInitialValues, multiple_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);
+  EXPECT_EQ(3u, list_params_result.names.size());
+  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_parameter.cpp

@@ -0,0 +1,711 @@
+// 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 <algorithm>
+#include <limits>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class TestParameter : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+TEST(TestParameter, not_set_variant) {
+  // Direct instantiation
+  rclcpp::Parameter not_set_variant;
+  EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, not_set_variant.get_type());
+  EXPECT_EQ("not set", not_set_variant.get_type_name());
+
+  EXPECT_THROW(not_set_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(not_set_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(not_set_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(not_set_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(not_set_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(not_set_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(not_set_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(not_set_variant.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(not_set_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  rcl_interfaces::msg::Parameter not_set_param = not_set_variant.to_parameter_msg();
+  EXPECT_EQ("", not_set_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET, not_set_param.value.type);
+
+  // From parameter message
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET,
+    rclcpp::Parameter::from_parameter_msg(not_set_param).get_type());
+}
+
+TEST(TestParameter, bool_variant) {
+  // Direct instantiation
+  rclcpp::Parameter bool_variant_true("bool_param", true);
+  EXPECT_EQ("bool_param", bool_variant_true.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL, bool_variant_true.get_type());
+  EXPECT_EQ("bool", bool_variant_true.get_type_name());
+  EXPECT_TRUE(bool_variant_true.get_value<rclcpp::ParameterType::PARAMETER_BOOL>());
+  EXPECT_TRUE(bool_variant_true.get_value_message().bool_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
+    bool_variant_true.get_value_message().type);
+  EXPECT_TRUE(bool_variant_true.as_bool());
+
+  EXPECT_THROW(bool_variant_true.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_variant_true.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_variant_true.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_variant_true.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_variant_true.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_variant_true.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_variant_true.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_variant_true.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ("true", bool_variant_true.value_to_string());
+
+  rclcpp::Parameter bool_variant_false("bool_param", false);
+  EXPECT_FALSE(bool_variant_false.get_value<rclcpp::ParameterType::PARAMETER_BOOL>());
+  EXPECT_FALSE(bool_variant_false.get_value_message().bool_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
+    bool_variant_false.get_value_message().type);
+
+  rcl_interfaces::msg::Parameter bool_param = bool_variant_true.to_parameter_msg();
+  EXPECT_EQ("bool_param", bool_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, bool_param.value.type);
+  EXPECT_TRUE(bool_param.value.bool_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg_true =
+    rclcpp::Parameter::from_parameter_msg(bool_param);
+  EXPECT_EQ("bool_param", from_msg_true.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL, from_msg_true.get_type());
+  EXPECT_EQ("bool", from_msg_true.get_type_name());
+  EXPECT_TRUE(from_msg_true.get_value<rclcpp::ParameterType::PARAMETER_BOOL>());
+  EXPECT_TRUE(from_msg_true.get_value_message().bool_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
+    bool_variant_false.get_value_message().type);
+
+  bool_param.value.bool_value = false;
+  rclcpp::Parameter from_msg_false =
+    rclcpp::Parameter::from_parameter_msg(bool_param);
+  EXPECT_FALSE(from_msg_false.get_value<rclcpp::ParameterType::PARAMETER_BOOL>());
+  EXPECT_FALSE(from_msg_false.get_value_message().bool_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
+    bool_variant_false.get_value_message().type);
+}
+
+TEST(TestParameter, integer_variant) {
+  const int TEST_VALUE {42};
+
+  // Direct instantiation
+  rclcpp::Parameter integer_variant("integer_param", TEST_VALUE);
+  EXPECT_EQ("integer_param", integer_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, integer_variant.get_type());
+  EXPECT_EQ("integer", integer_variant.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    integer_variant.get_value<rclcpp::ParameterType::PARAMETER_INTEGER>());
+  EXPECT_EQ(TEST_VALUE, integer_variant.get_value_message().integer_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
+    integer_variant.get_value_message().type);
+  EXPECT_EQ(TEST_VALUE, integer_variant.as_int());
+
+  EXPECT_THROW(integer_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_variant.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ("42", integer_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter integer_param = integer_variant.to_parameter_msg();
+  EXPECT_EQ("integer_param", integer_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, integer_param.value.type);
+  EXPECT_EQ(TEST_VALUE, integer_param.value.integer_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(integer_param);
+  EXPECT_EQ("integer_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, from_msg.get_type());
+  EXPECT_EQ("integer", from_msg.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    from_msg.get_value<rclcpp::ParameterType::PARAMETER_INTEGER>());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
+    from_msg.get_value_message().type);
+}
+
+TEST(TestParameter, long_integer_variant) {
+  const int64_t TEST_VALUE {std::numeric_limits<int64_t>::max()};
+
+  // Direct instantiation
+  rclcpp::Parameter long_variant("long_integer_param", TEST_VALUE);
+  EXPECT_EQ("long_integer_param", long_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, long_variant.get_type());
+  EXPECT_EQ("integer", long_variant.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    long_variant.get_value<rclcpp::ParameterType::PARAMETER_INTEGER>());
+  EXPECT_EQ(TEST_VALUE, long_variant.get_value_message().integer_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
+    long_variant.get_value_message().type);
+  EXPECT_EQ(TEST_VALUE, long_variant.as_int());
+
+  EXPECT_THROW(long_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_variant.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ("9223372036854775807", long_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter integer_param = long_variant.to_parameter_msg();
+  EXPECT_EQ("long_integer_param", integer_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, integer_param.value.type);
+  EXPECT_EQ(TEST_VALUE, integer_param.value.integer_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(integer_param);
+  EXPECT_EQ("long_integer_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, from_msg.get_type());
+  EXPECT_EQ("integer", from_msg.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    from_msg.get_value<rclcpp::ParameterType::PARAMETER_INTEGER>());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER,
+    from_msg.get_value_message().type);
+}
+
+TEST(TestParameter, float_variant) {
+  const float TEST_VALUE {42.0f};
+
+  // Direct instantiation
+  rclcpp::Parameter float_variant("float_param", TEST_VALUE);
+  EXPECT_EQ("float_param", float_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, float_variant.get_type());
+  EXPECT_EQ("double", float_variant.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    float_variant.get_value<rclcpp::ParameterType::PARAMETER_DOUBLE>());
+  EXPECT_EQ(TEST_VALUE, float_variant.get_value_message().double_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
+    float_variant.get_value_message().type);
+  EXPECT_EQ(TEST_VALUE, float_variant.as_double());
+
+  EXPECT_THROW(float_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_variant.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ("42.000000", float_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter float_param = float_variant.to_parameter_msg();
+  EXPECT_EQ("float_param", float_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, float_param.value.type);
+  EXPECT_EQ(TEST_VALUE, float_param.value.double_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(float_param);
+  EXPECT_EQ("float_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, from_msg.get_type());
+  EXPECT_EQ("double", from_msg.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    from_msg.get_value<rclcpp::ParameterType::PARAMETER_DOUBLE>());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
+    from_msg.get_value_message().type);
+}
+
+TEST(TestParameter, double_variant) {
+  const double TEST_VALUE {-42.1};
+
+  // Direct instantiation
+  rclcpp::Parameter double_variant("double_param", TEST_VALUE);
+  EXPECT_EQ("double_param", double_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, double_variant.get_type());
+  EXPECT_EQ("double", double_variant.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    double_variant.get_value<rclcpp::ParameterType::PARAMETER_DOUBLE>());
+  EXPECT_EQ(TEST_VALUE, double_variant.get_value_message().double_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
+    double_variant.get_value_message().type);
+  EXPECT_EQ(TEST_VALUE, double_variant.as_double());
+
+  EXPECT_THROW(double_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_variant.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ("-42.100000", double_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter double_param = double_variant.to_parameter_msg();
+  EXPECT_EQ("double_param", double_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, double_param.value.type);
+  EXPECT_EQ(TEST_VALUE, double_param.value.double_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(double_param);
+  EXPECT_EQ("double_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, from_msg.get_type());
+  EXPECT_EQ("double", from_msg.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    from_msg.get_value<rclcpp::ParameterType::PARAMETER_DOUBLE>());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE,
+    from_msg.get_value_message().type);
+}
+
+TEST(TestParameter, string_variant) {
+  const std::string TEST_VALUE {"ROS2"};
+
+  // Direct instantiation
+  rclcpp::Parameter string_variant("string_param", TEST_VALUE);
+  EXPECT_EQ("string_param", string_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING, string_variant.get_type());
+  EXPECT_EQ("string", string_variant.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    string_variant.get_value<rclcpp::ParameterType::PARAMETER_STRING>());
+  EXPECT_EQ(TEST_VALUE, string_variant.get_value_message().string_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
+    string_variant.get_value_message().type);
+  EXPECT_EQ(TEST_VALUE, string_variant.as_string());
+
+  EXPECT_THROW(string_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_variant.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ(TEST_VALUE, string_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter string_param = string_variant.to_parameter_msg();
+  EXPECT_EQ("string_param", string_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING, string_param.value.type);
+  EXPECT_EQ(TEST_VALUE, string_param.value.string_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(string_param);
+  EXPECT_EQ("string_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING, from_msg.get_type());
+  EXPECT_EQ("string", from_msg.get_type_name());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value<rclcpp::ParameterType::PARAMETER_STRING>());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().string_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING,
+    from_msg.get_value_message().type);
+}
+
+TEST(TestParameter, byte_array_variant) {
+  const std::vector<uint8_t> TEST_VALUE {0x52, 0x4f, 0x53, 0x32};
+
+  // Direct instantiation
+  rclcpp::Parameter byte_array_variant("byte_array_param", TEST_VALUE);
+  EXPECT_EQ("byte_array_param", byte_array_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BYTE_ARRAY, byte_array_variant.get_type());
+  EXPECT_EQ("byte_array", byte_array_variant.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    byte_array_variant.get_value<rclcpp::ParameterType::PARAMETER_BYTE_ARRAY>());
+  EXPECT_EQ(TEST_VALUE, byte_array_variant.get_value_message().byte_array_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY,
+    byte_array_variant.get_value_message().type);
+  EXPECT_EQ(TEST_VALUE, byte_array_variant.as_byte_array());
+
+  EXPECT_THROW(byte_array_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(byte_array_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(byte_array_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(byte_array_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(byte_array_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(byte_array_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(byte_array_variant.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(byte_array_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ("[0x52, 0x4f, 0x53, 0x32]", byte_array_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter byte_array_param = byte_array_variant.to_parameter_msg();
+  EXPECT_EQ("byte_array_param", byte_array_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, byte_array_param.value.type);
+  EXPECT_EQ(TEST_VALUE, byte_array_param.value.byte_array_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(byte_array_param);
+  EXPECT_EQ("byte_array_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BYTE_ARRAY, from_msg.get_type());
+  EXPECT_EQ("byte_array", from_msg.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    from_msg.get_value<rclcpp::ParameterType::PARAMETER_BYTE_ARRAY>());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().byte_array_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY,
+    from_msg.get_value_message().type);
+}
+
+TEST(TestParameter, bool_array_variant) {
+  const std::vector<bool> TEST_VALUE {false, true, true, false, false, true};
+
+  // Direct instantiation
+  rclcpp::Parameter bool_array_variant("bool_array_param", TEST_VALUE);
+  EXPECT_EQ("bool_array_param", bool_array_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL_ARRAY, bool_array_variant.get_type());
+  EXPECT_EQ("bool_array", bool_array_variant.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    bool_array_variant.get_value<rclcpp::ParameterType::PARAMETER_BOOL_ARRAY>());
+  EXPECT_EQ(TEST_VALUE, bool_array_variant.get_value_message().bool_array_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY,
+    bool_array_variant.get_value_message().type);
+  EXPECT_EQ(TEST_VALUE, bool_array_variant.as_bool_array());
+
+  EXPECT_THROW(bool_array_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_array_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_array_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_array_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_array_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_array_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_array_variant.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(bool_array_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ("[false, true, true, false, false, true]", bool_array_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter bool_array_param = bool_array_variant.to_parameter_msg();
+  EXPECT_EQ("bool_array_param", bool_array_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, bool_array_param.value.type);
+  EXPECT_EQ(TEST_VALUE, bool_array_param.value.bool_array_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(bool_array_param);
+  EXPECT_EQ("bool_array_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL_ARRAY, from_msg.get_type());
+  EXPECT_EQ("bool_array", from_msg.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    from_msg.get_value<rclcpp::ParameterType::PARAMETER_BOOL_ARRAY>());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().bool_array_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY,
+    from_msg.get_value_message().type);
+}
+
+TEST(TestParameter, integer_array_variant) {
+  const std::vector<int> TEST_VALUE
+  {42, -99, std::numeric_limits<int>::max(), std::numeric_limits<int>::lowest(), 0};
+
+  // Direct instantiation
+  rclcpp::Parameter integer_array_variant("integer_array_param", TEST_VALUE);
+
+  EXPECT_EQ("integer_array_param", integer_array_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY,
+    integer_array_variant.get_type());
+  EXPECT_EQ("integer_array", integer_array_variant.get_type_name());
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
+    integer_array_variant.get_value_message().type);
+
+  // No direct comparison of vectors of ints and long ints
+  const auto & param_value_ref =
+    integer_array_variant.get_value<rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY>();
+  auto mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value_ref.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value_ref.end(), mismatches.second);
+
+  auto param_value = integer_array_variant.get_value_message().integer_array_value;
+  mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value.end(), mismatches.second);
+
+  param_value = integer_array_variant.as_integer_array();
+  mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value.end(), mismatches.second);
+
+  EXPECT_THROW(integer_array_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_array_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_array_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_array_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_array_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_array_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_array_variant.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(integer_array_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ(
+    "[42, -99, 2147483647, -2147483648, 0]",
+    integer_array_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter integer_array_param = integer_array_variant.to_parameter_msg();
+  EXPECT_EQ("integer_array_param", integer_array_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
+    integer_array_param.value.type);
+
+  param_value = integer_array_param.value.integer_array_value;
+  mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value.end(), mismatches.second);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(integer_array_param);
+  EXPECT_EQ("integer_array_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_type());
+  EXPECT_EQ("integer_array", from_msg.get_type_name());
+
+  param_value = from_msg.get_value<rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY>();
+  mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value.end(), mismatches.second);
+
+  param_value = from_msg.get_value_message().integer_array_value;
+  mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value.end(), mismatches.second);
+
+  EXPECT_EQ(from_msg.get_value_message().type,
+    rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY);
+}
+
+TEST(TestParameter, long_integer_array_variant) {
+  const std::vector<int64_t> TEST_VALUE
+  {42, -99, std::numeric_limits<int64_t>::max(), std::numeric_limits<int64_t>::lowest(), 0};
+
+  rclcpp::Parameter long_array_variant("long_integer_array_param", TEST_VALUE);
+  EXPECT_EQ("long_integer_array_param", long_array_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY,
+    long_array_variant.get_type());
+  EXPECT_EQ("integer_array", long_array_variant.get_type_name());
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
+    long_array_variant.get_value_message().type);
+  EXPECT_EQ(TEST_VALUE,
+    long_array_variant.get_value<rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY>());
+  EXPECT_EQ(TEST_VALUE, long_array_variant.get_value_message().integer_array_value);
+  EXPECT_EQ(TEST_VALUE, long_array_variant.as_integer_array());
+
+  EXPECT_THROW(long_array_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_array_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_array_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_array_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_array_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_array_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_array_variant.as_double_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(long_array_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ(
+    "[42, -99, 9223372036854775807, -9223372036854775808, 0]",
+    long_array_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter integer_array_param = long_array_variant.to_parameter_msg();
+  EXPECT_EQ("long_integer_array_param", integer_array_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
+    integer_array_param.value.type);
+  EXPECT_EQ(TEST_VALUE, integer_array_param.value.integer_array_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(integer_array_param);
+  EXPECT_EQ("long_integer_array_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_type());
+  EXPECT_EQ("integer_array", from_msg.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    from_msg.get_value<rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY>());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_array_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY,
+    from_msg.get_value_message().type);
+}
+
+TEST(TestParameter, float_array_variant) {
+  const std::vector<float> TEST_VALUE
+  {42.1f, -99.1f, std::numeric_limits<float>::max(), std::numeric_limits<float>::lowest(), 0.1f};
+
+  // Direct instantiation
+  rclcpp::Parameter float_array_variant("float_array_param", TEST_VALUE);
+
+  EXPECT_EQ("float_array_param", float_array_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY,
+    float_array_variant.get_type());
+  EXPECT_EQ("double_array", float_array_variant.get_type_name());
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
+    float_array_variant.get_value_message().type);
+
+  // No direct comparison of vectors of floats and doubles
+  const auto & param_value_ref =
+    float_array_variant.get_value<rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY>();
+  auto mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value_ref.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value_ref.end(), mismatches.second);
+
+  auto param_value = float_array_variant.get_value_message().double_array_value;
+  mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value.end(), mismatches.second);
+
+  param_value = float_array_variant.as_double_array();
+  mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value.end(), mismatches.second);
+
+  EXPECT_THROW(float_array_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_array_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_array_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_array_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_array_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_array_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_array_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(float_array_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ(
+    "[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]",
+    float_array_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter float_array_param = float_array_variant.to_parameter_msg();
+  EXPECT_EQ("float_array_param", float_array_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
+    float_array_param.value.type);
+
+  param_value = float_array_param.value.double_array_value;
+  mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value.end(), mismatches.second);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(float_array_param);
+  EXPECT_EQ("float_array_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_type());
+  EXPECT_EQ("double_array", from_msg.get_type_name());
+
+  param_value = from_msg.get_value<rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY>();
+  mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value.end(), mismatches.second);
+
+  param_value = from_msg.get_value_message().double_array_value;
+  mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin());
+  EXPECT_EQ(TEST_VALUE.end(), mismatches.first);
+  EXPECT_EQ(param_value.end(), mismatches.second);
+
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
+    from_msg.get_value_message().type);
+}
+
+TEST(TestParameter, double_array_variant) {
+  const std::vector<double> TEST_VALUE
+  {42.1, -99.1, std::numeric_limits<double>::max(), std::numeric_limits<double>::lowest(), 0.1};
+
+  rclcpp::Parameter double_array_variant("double_array_param", TEST_VALUE);
+  EXPECT_EQ("double_array_param", double_array_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY,
+    double_array_variant.get_type());
+  EXPECT_EQ("double_array", double_array_variant.get_type_name());
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
+    double_array_variant.get_value_message().type);
+  EXPECT_EQ(TEST_VALUE,
+    double_array_variant.get_value<rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY>());
+  EXPECT_EQ(TEST_VALUE, double_array_variant.get_value_message().double_array_value);
+  EXPECT_EQ(TEST_VALUE, double_array_variant.as_double_array());
+
+  EXPECT_THROW(double_array_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_array_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_array_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_array_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_array_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_array_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_array_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(double_array_variant.as_string_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ(
+    "[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]",
+    double_array_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter double_array_param = double_array_variant.to_parameter_msg();
+  EXPECT_EQ("double_array_param", double_array_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
+    double_array_param.value.type);
+  EXPECT_EQ(TEST_VALUE, double_array_param.value.double_array_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(double_array_param);
+  EXPECT_EQ("double_array_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_type());
+  EXPECT_EQ("double_array", from_msg.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    from_msg.get_value<rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY>());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_array_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY,
+    from_msg.get_value_message().type);
+}
+
+TEST(TestParameter, string_array_variant) {
+  const std::vector<std::string> TEST_VALUE {"R", "O", "S2"};
+
+  // Direct instantiation
+  rclcpp::Parameter string_array_variant("string_array_param", TEST_VALUE);
+  EXPECT_EQ("string_array_param", string_array_variant.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING_ARRAY,
+    string_array_variant.get_type());
+  EXPECT_EQ("string_array", string_array_variant.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    string_array_variant.get_value<rclcpp::ParameterType::PARAMETER_STRING_ARRAY>());
+  EXPECT_EQ(TEST_VALUE, string_array_variant.get_value_message().string_array_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY,
+    string_array_variant.get_value_message().type);
+  EXPECT_EQ(TEST_VALUE, string_array_variant.as_string_array());
+
+  EXPECT_THROW(string_array_variant.as_bool(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_array_variant.as_int(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_array_variant.as_double(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_array_variant.as_string(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_array_variant.as_byte_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_array_variant.as_bool_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_array_variant.as_integer_array(), rclcpp::ParameterTypeException);
+  EXPECT_THROW(string_array_variant.as_double_array(), rclcpp::ParameterTypeException);
+
+  EXPECT_EQ("[R, O, S2]", string_array_variant.value_to_string());
+
+  rcl_interfaces::msg::Parameter string_array_param = string_array_variant.to_parameter_msg();
+  EXPECT_EQ("string_array_param", string_array_param.name);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY,
+    string_array_param.value.type);
+  EXPECT_EQ(TEST_VALUE, string_array_param.value.string_array_value);
+
+  // From parameter message
+  rclcpp::Parameter from_msg =
+    rclcpp::Parameter::from_parameter_msg(string_array_param);
+  EXPECT_EQ("string_array_param", from_msg.get_name());
+  EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING_ARRAY, from_msg.get_type());
+  EXPECT_EQ("string_array", from_msg.get_type_name());
+  EXPECT_EQ(TEST_VALUE,
+    from_msg.get_value<rclcpp::ParameterType::PARAMETER_STRING_ARRAY>());
+  EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().string_array_value);
+  EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY,
+    from_msg.get_value_message().type);
+}

rclcpp/test/test_parameter_map.cpp

@@ -0,0 +1,354 @@
+// 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 <rcl_yaml_param_parser/parser.h>
+#include <rcutils/strdup.h>
+
+#include <cstdio>
+#include <string>
+#include <vector>
+
+#include "rclcpp/parameter_map.hpp"
+
+rcl_params_t *
+make_params(std::vector<std::string> node_names)
+{
+  rcl_allocator_t alloc = rcl_get_default_allocator();
+  rcl_params_t * c_params = rcl_yaml_node_struct_init(alloc);
+  c_params->num_nodes = node_names.size();
+  c_params->allocator = alloc;
+  if (c_params->num_nodes) {
+    // Copy node names
+    for (size_t n = 0; n < node_names.size(); ++n) {
+      c_params->node_names[n] = static_cast<char *>(alloc.allocate(
+          sizeof(char) * (node_names[n].size() + 1), alloc.state));
+      std::snprintf(c_params->node_names[n], node_names[n].size() + 1, "%s", node_names[n].c_str());
+    }
+    // zero init node params
+    for (size_t n = 0; n < node_names.size(); ++n) {
+      c_params->params[n].parameter_names = NULL;
+      c_params->params[n].parameter_values = NULL;
+      c_params->params[n].num_params = 0;
+    }
+  }
+  return c_params;
+}
+
+void
+make_node_params(rcl_params_t * c_params, size_t node_idx, std::vector<std::string> param_names)
+{
+  rcl_allocator_t alloc = c_params->allocator;
+  ASSERT_LT(node_idx, c_params->num_nodes);
+  ASSERT_GT(param_names.size(), 0u);
+
+  rcl_node_params_s * c_node_params = &(c_params->params[node_idx]);
+  c_node_params->num_params = param_names.size();
+
+  // Copy parameter names
+  c_node_params->parameter_names = static_cast<char **>(
+    alloc.allocate(sizeof(char *) * param_names.size(), alloc.state));
+  for (size_t p = 0; p < param_names.size(); ++p) {
+    const std::string & param_name = param_names[p];
+    c_node_params->parameter_names[p] = static_cast<char *>(alloc.allocate(
+        sizeof(char) * (param_name.size() + 1), alloc.state));
+    std::snprintf(
+      c_node_params->parameter_names[p], param_name.size() + 1, "%s", param_name.c_str());
+  }
+  // zero init parameter value
+  c_node_params->parameter_values = static_cast<rcl_variant_t *>(alloc.allocate(
+      sizeof(rcl_variant_t) * param_names.size(), alloc.state));
+  for (size_t p = 0; p < param_names.size(); ++p) {
+    c_node_params->parameter_values[p].bool_value = NULL;
+    c_node_params->parameter_values[p].integer_value = NULL;
+    c_node_params->parameter_values[p].double_value = NULL;
+    c_node_params->parameter_values[p].string_value = NULL;
+    c_node_params->parameter_values[p].byte_array_value = NULL;
+    c_node_params->parameter_values[p].bool_array_value = NULL;
+    c_node_params->parameter_values[p].integer_array_value = NULL;
+    c_node_params->parameter_values[p].double_array_value = NULL;
+    c_node_params->parameter_values[p].string_array_value = NULL;
+  }
+}
+
+TEST(Test_parameter_map_from, null_c_parameter)
+{
+  EXPECT_THROW(rclcpp::parameter_map_from(NULL), rclcpp::exceptions::InvalidParametersException);
+}
+
+TEST(Test_parameter_map_from, null_node_names)
+{
+  rcl_params_t * c_params = make_params({});
+  c_params->num_nodes = 1;
+
+  EXPECT_THROW(
+    rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException);
+
+  c_params->num_nodes = 0;
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, null_node_params)
+{
+  rcl_params_t * c_params = make_params({"foo"});
+  std::snprintf(c_params->node_names[0], 3 + 1, "foo");
+  auto allocated_params = c_params->params;
+  c_params->params = NULL;
+
+  EXPECT_THROW(
+    rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException);
+
+  c_params->params = allocated_params;
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, null_node_name_in_node_names)
+{
+  rcl_params_t * c_params = make_params({"foo"});
+  auto allocated_name = c_params->node_names[0];
+  c_params->node_names[0] = NULL;
+
+  EXPECT_THROW(
+    rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException);
+
+  c_params->node_names[0] = allocated_name;
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, null_node_param_value)
+{
+  rcl_params_t * c_params = make_params({"foo"});
+  make_node_params(c_params, 0, {"bar"});
+
+  EXPECT_THROW(
+    rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParameterValueException);
+
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, null_node_param_name)
+{
+  rcl_params_t * c_params = make_params({"foo"});
+  make_node_params(c_params, 0, {"bar"});
+  auto allocated_name = c_params->params[0].parameter_names[0];
+  c_params->params[0].parameter_names[0] = NULL;
+
+  EXPECT_THROW(
+    rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException);
+
+  c_params->params[0].parameter_names[0] = allocated_name;
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, bool_param_value)
+{
+  rcl_params_t * c_params = make_params({"foo"});
+  make_node_params(c_params, 0, {"true_bool", "false_bool"});
+  bool true_bool = true;
+  bool false_bool = false;
+  c_params->params[0].parameter_values[0].bool_value = &true_bool;
+  c_params->params[0].parameter_values[1].bool_value = &false_bool;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params);
+  const std::vector<rclcpp::Parameter> & params = map.at("/foo");
+  EXPECT_STREQ("true_bool", params.at(0).get_name().c_str());
+  EXPECT_TRUE(params.at(0).get_value<bool>());
+  EXPECT_STREQ("false_bool", params.at(1).get_name().c_str());
+  EXPECT_FALSE(params.at(1).get_value<bool>());
+
+  c_params->params[0].parameter_values[0].bool_value = NULL;
+  c_params->params[0].parameter_values[1].bool_value = NULL;
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, integer_param_value)
+{
+  rcl_params_t * c_params = make_params({"bar"});
+  make_node_params(c_params, 0, {"positive.int", "negative.int"});
+  int64_t positive_int = 42;
+  int64_t negative_int = -12345;
+  c_params->params[0].parameter_values[0].integer_value = &positive_int;
+  c_params->params[0].parameter_values[1].integer_value = &negative_int;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params);
+  const std::vector<rclcpp::Parameter> & params = map.at("/bar");
+  EXPECT_STREQ("positive.int", params.at(0).get_name().c_str());
+  EXPECT_EQ(42, params.at(0).get_value<int64_t>());
+  EXPECT_STREQ("negative.int", params.at(1).get_name().c_str());
+  EXPECT_EQ(-12345, params.at(1).get_value<int64_t>());
+
+  c_params->params[0].parameter_values[0].integer_value = NULL;
+  c_params->params[0].parameter_values[1].integer_value = NULL;
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, double_param_value)
+{
+  rcl_params_t * c_params = make_params({"foo/bar"});
+  make_node_params(c_params, 0, {"positive.double", "negative.double"});
+  double positive_double = 3.14;
+  double negative_double = -2.718;
+  c_params->params[0].parameter_values[0].double_value = &positive_double;
+  c_params->params[0].parameter_values[1].double_value = &negative_double;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params);
+  const std::vector<rclcpp::Parameter> & params = map.at("/foo/bar");
+  EXPECT_STREQ("positive.double", params.at(0).get_name().c_str());
+  EXPECT_DOUBLE_EQ(3.14, params.at(0).get_value<double>());
+  EXPECT_STREQ("negative.double", params.at(1).get_name().c_str());
+  EXPECT_DOUBLE_EQ(-2.718, params.at(1).get_value<double>());
+
+  c_params->params[0].parameter_values[0].double_value = NULL;
+  c_params->params[0].parameter_values[1].double_value = NULL;
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, string_param_value)
+{
+  rcl_params_t * c_params = make_params({"/foo/bar"});
+  make_node_params(c_params, 0, {"string_param"});
+  std::string hello_world = "hello world";
+  char * c_hello_world = new char[hello_world.length() + 1];
+  std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str());
+  c_params->params[0].parameter_values[0].string_value = c_hello_world;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params);
+  const std::vector<rclcpp::Parameter> & params = map.at("/foo/bar");
+  EXPECT_STREQ("string_param", params.at(0).get_name().c_str());
+  EXPECT_STREQ(hello_world.c_str(), params.at(0).get_value<std::string>().c_str());
+
+  c_params->params[0].parameter_values[0].string_value = NULL;
+  delete[] c_hello_world;
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+#define MAKE_ARRAY_VALUE(VAR, TYPE, V1, V2) \
+  do { \
+    VAR.values = new TYPE[2]; \
+    VAR.size = 2; \
+    VAR.values[0] = V1; \
+    VAR.values[1] = V2; \
+  } while (false)
+
+#define FREE_ARRAY_VALUE(VAR) \
+  do { \
+    delete[] VAR.values; \
+  } while (false)
+
+TEST(Test_parameter_map_from, byte_array_param_value)
+{
+  rcl_params_t * c_params = make_params({"/foobar"});
+  make_node_params(c_params, 0, {"byte_array_param"});
+  rcl_byte_array_t c_byte_array;
+  MAKE_ARRAY_VALUE(c_byte_array, uint8_t, 0xf0, 0xaa);
+  c_params->params[0].parameter_values[0].byte_array_value = &c_byte_array;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params);
+  const std::vector<rclcpp::Parameter> & params = map.at("/foobar");
+  EXPECT_STREQ("byte_array_param", params.at(0).get_name().c_str());
+  std::vector<uint8_t> byte_array = params.at(0).get_value<std::vector<uint8_t>>();
+  ASSERT_EQ(2u, byte_array.size());
+  EXPECT_EQ(0xf0, byte_array.at(0));
+  EXPECT_EQ(0xaa, byte_array.at(1));
+
+  c_params->params[0].parameter_values[0].byte_array_value = NULL;
+  FREE_ARRAY_VALUE(c_byte_array);
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, bool_array_param_value)
+{
+  rcl_params_t * c_params = make_params({"foo/bar/baz"});
+  make_node_params(c_params, 0, {"bool_array_param"});
+  rcl_bool_array_t c_bool_array;
+  MAKE_ARRAY_VALUE(c_bool_array, bool, true, false);
+  c_params->params[0].parameter_values[0].bool_array_value = &c_bool_array;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params);
+  const std::vector<rclcpp::Parameter> & params = map.at("/foo/bar/baz");
+  EXPECT_STREQ("bool_array_param", params.at(0).get_name().c_str());
+  std::vector<bool> bool_array = params.at(0).get_value<std::vector<bool>>();
+  ASSERT_EQ(2u, bool_array.size());
+  EXPECT_TRUE(bool_array.at(0));
+  EXPECT_FALSE(bool_array.at(1));
+
+  c_params->params[0].parameter_values[0].bool_array_value = NULL;
+  FREE_ARRAY_VALUE(c_bool_array);
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, integer_array_param_value)
+{
+  rcl_params_t * c_params = make_params({"foo"});
+  make_node_params(c_params, 0, {"integer_array_param"});
+  rcl_int64_array_t c_integer_array;
+  MAKE_ARRAY_VALUE(c_integer_array, int64_t, 42, -12345);
+  c_params->params[0].parameter_values[0].integer_array_value = &c_integer_array;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params);
+  const std::vector<rclcpp::Parameter> & params = map.at("/foo");
+  EXPECT_STREQ("integer_array_param", params.at(0).get_name().c_str());
+  std::vector<int64_t> integer_array = params.at(0).get_value<std::vector<int64_t>>();
+  ASSERT_EQ(2u, integer_array.size());
+  EXPECT_EQ(42, integer_array.at(0));
+  EXPECT_EQ(-12345, integer_array.at(1));
+
+  c_params->params[0].parameter_values[0].integer_array_value = NULL;
+  FREE_ARRAY_VALUE(c_integer_array);
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, double_array_param_value)
+{
+  rcl_params_t * c_params = make_params({"foo"});
+  make_node_params(c_params, 0, {"double_array_param"});
+  rcl_double_array_t c_double_array;
+  MAKE_ARRAY_VALUE(c_double_array, double, 3.14, -2.718);
+  c_params->params[0].parameter_values[0].double_array_value = &c_double_array;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params);
+  const std::vector<rclcpp::Parameter> & params = map.at("/foo");
+  EXPECT_STREQ("double_array_param", params.at(0).get_name().c_str());
+  std::vector<double> double_array = params.at(0).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));
+
+  c_params->params[0].parameter_values[0].double_array_value = NULL;
+  FREE_ARRAY_VALUE(c_double_array);
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, string_array_param_value)
+{
+  rcl_params_t * c_params = make_params({"foo"});
+  make_node_params(c_params, 0, {"string_array_param"});
+  rcutils_string_array_t c_string_array = rcutils_get_zero_initialized_string_array();
+  ASSERT_EQ(RCUTILS_RET_OK, rcutils_string_array_init(&c_string_array, 2, &(c_params->allocator)));
+  c_string_array.data[0] = rcutils_strdup("Hello", c_params->allocator);
+  c_string_array.data[1] = rcutils_strdup("World", c_params->allocator);
+  c_params->params[0].parameter_values[0].string_array_value = &c_string_array;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params);
+  const std::vector<rclcpp::Parameter> & params = map.at("/foo");
+  EXPECT_STREQ("string_array_param", params.at(0).get_name().c_str());
+  std::vector<std::string> string_array = params.at(0).get_value<std::vector<std::string>>();
+  ASSERT_EQ(2u, string_array.size());
+  EXPECT_STREQ("Hello", string_array.at(0).c_str());
+  EXPECT_STREQ("World", string_array.at(1).c_str());
+
+  EXPECT_EQ(RCUTILS_RET_OK, rcutils_string_array_fini(&c_string_array));
+  c_params->params[0].parameter_values[0].string_array_value = NULL;
+  rcl_yaml_node_struct_fini(c_params);
+}

rclcpp/test/test_serialized_message_allocator.cpp

@@ -0,0 +1,67 @@
+// 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 "rclcpp/rclcpp.hpp"
+
+#include "rcl/types.h"
+
+#include "test_msgs/msg/empty.hpp"
+
+TEST(TestSerializedMessageAllocator, default_allocator) {
+  using DummyMessageT = float;
+  auto mem_strategy =
+    rclcpp::message_memory_strategy::MessageMemoryStrategy<DummyMessageT>::create_default();
+
+  auto msg0 = mem_strategy->borrow_serialized_message();
+  ASSERT_EQ(msg0->buffer_capacity, 0u);
+  mem_strategy->return_serialized_message(msg0);
+
+  auto msg100 = mem_strategy->borrow_serialized_message(100);
+  ASSERT_EQ(msg100->buffer_capacity, 100u);
+  mem_strategy->return_serialized_message(msg100);
+
+  auto msg200 = mem_strategy->borrow_serialized_message();
+  auto ret = rmw_serialized_message_resize(msg200.get(), 200);
+  ASSERT_EQ(RCL_RET_OK, ret);
+  EXPECT_EQ(0u, msg200->buffer_length);
+  EXPECT_EQ(200u, msg200->buffer_capacity);
+  mem_strategy->return_serialized_message(msg200);
+
+  auto msg1000 = mem_strategy->borrow_serialized_message(1000);
+  ASSERT_EQ(msg1000->buffer_capacity, 1000u);
+  ret = rmw_serialized_message_resize(msg1000.get(), 2000);
+  ASSERT_EQ(RCL_RET_OK, ret);
+  EXPECT_EQ(2000u, msg1000->buffer_capacity);
+  mem_strategy->return_serialized_message(msg1000);
+}
+
+TEST(TestSerializedMessageAllocator, borrow_from_subscription) {
+  rclcpp::init(0, NULL);
+
+  auto node = std::make_shared<rclcpp::Node>("test_serialized_message_allocator_node");
+  std::shared_ptr<rclcpp::SubscriptionBase> sub =
+    node->create_subscription<test_msgs::msg::Empty>("~/dummy_topic", [](
+        std::shared_ptr<test_msgs::msg::Empty> test_msg) {(void) test_msg;});
+
+  auto msg0 = sub->create_serialized_message();
+  EXPECT_EQ(0u, msg0->buffer_capacity);
+  sub->return_serialized_message(msg0);
+
+  rclcpp::shutdown();
+}

rclcpp/test/test_subscription.cpp

@@ -24,6 +24,12 @@
 
 class TestSubscription : public ::testing::Test
 {
+public:
+  void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg)
+  {
+    (void)msg;
+  }
+
 protected:
   static void SetUpTestCase()
   {
@@ -32,7 +38,7 @@ protected:
 
   void SetUp()
   {
-    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    node = std::make_shared<rclcpp::Node>("test_subscription", "/ns");
   }
 
   void TearDown()
@@ -43,6 +49,45 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
+class SubscriptionClassNodeInheritance : public rclcpp::Node
+{
+public:
+  SubscriptionClassNodeInheritance()
+  : Node("subscription_class_node_inheritance")
+  {
+  }
+
+  void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg)
+  {
+    (void)msg;
+  }
+
+  void CreateSubscription()
+  {
+    auto callback = std::bind(
+      &SubscriptionClassNodeInheritance::OnMessage, this, std::placeholders::_1);
+    using rcl_interfaces::msg::IntraProcessMessage;
+    auto sub = this->create_subscription<IntraProcessMessage>("topic", callback);
+  }
+};
+
+class SubscriptionClass
+{
+public:
+  void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg)
+  {
+    (void)msg;
+  }
+
+  void CreateSubscription()
+  {
+    auto node = std::make_shared<rclcpp::Node>("test_subscription_member_callback", "/ns");
+    auto callback = std::bind(&SubscriptionClass::OnMessage, this, std::placeholders::_1);
+    using rcl_interfaces::msg::IntraProcessMessage;
+    auto sub = node->create_subscription<IntraProcessMessage>("topic", callback);
+  }
+};
+
 /*
    Testing subscription construction and destruction.
  */
@@ -61,3 +106,27 @@ TEST_F(TestSubscription, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidTopicNameError);
   }
 }
+
+/*
+   Testing subscriptions using std::bind.
+ */
+TEST_F(TestSubscription, callback_bind) {
+  using rcl_interfaces::msg::IntraProcessMessage;
+  {
+    // Member callback for plain class
+    SubscriptionClass subscriptionObject;
+    subscriptionObject.CreateSubscription();
+  }
+  {
+    // Member callback for class inheriting from rclcpp::Node
+    SubscriptionClassNodeInheritance subscriptionObject;
+    subscriptionObject.CreateSubscription();
+  }
+  {
+    // Member callback for class inheriting from testing::Test
+    // Regression test for https://github.com/ros2/rclcpp/issues/479 where the TEST_F GTest macro
+    // was interfering with rclcpp's `function_traits`.
+    auto callback = std::bind(&TestSubscription::OnMessage, this, std::placeholders::_1);
+    auto sub = node->create_subscription<IntraProcessMessage>("topic", callback);
+  }
+}

rclcpp/test/test_subscription_traits.cpp

@@ -0,0 +1,178 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rcl/types.h"
+
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/subscription_traits.hpp"
+
+#include "test_msgs/msg/empty.hpp"
+
+void serialized_callback_copy(rcl_serialized_message_t unused)
+{
+  (void) unused;
+}
+
+void serialized_callback_shared_ptr(std::shared_ptr<rcl_serialized_message_t> unused)
+{
+  (void) unused;
+}
+
+void not_serialized_callback(char * unused)
+{
+  (void) unused;
+}
+
+void not_serialized_shared_ptr_callback(std::shared_ptr<char> unused)
+{
+  (void) unused;
+}
+
+void not_serialized_unique_ptr_callback(
+  test_msgs::msg::Empty::UniquePtrWithDeleter<rclcpp::allocator::Deleter<std::allocator<void>,
+  test_msgs::msg::Empty>> unused)
+{
+  (void) unused;
+}
+
+TEST(TestSubscriptionTraits, is_serialized_callback) {
+  // Test regular functions
+  auto cb1 = &serialized_callback_copy;
+  static_assert(
+    rclcpp::subscription_traits::is_serialized_callback<decltype(cb1)>::value == true,
+    "rcl_serialized_message_t in a first argument callback makes it a serialized callback");
+
+  auto cb2 = &serialized_callback_shared_ptr;
+  static_assert(
+    rclcpp::subscription_traits::is_serialized_callback<decltype(cb2)>::value == true,
+    "std::shared_ptr<rcl_serialized_message_t> in a callback makes it a serialized callback");
+
+  auto cb3 = &not_serialized_callback;
+  static_assert(
+    rclcpp::subscription_traits::is_serialized_callback<decltype(cb3)>::value == false,
+    "passing a char * is not a serialized callback");
+
+  auto cb4 = &not_serialized_shared_ptr_callback;
+  static_assert(
+    rclcpp::subscription_traits::is_serialized_callback<decltype(cb4)>::value == false,
+    "passing a std::shared_tr<char> is not a serialized callback");
+
+  auto cb5 = [](rcl_serialized_message_t unused) -> void
+    {
+      (void) unused;
+    };
+  static_assert(
+    rclcpp::subscription_traits::is_serialized_callback<decltype(cb5)>::value == true,
+    "rcl_serialized_message_t in a first argument callback makes it a serialized callback");
+
+  using MessageT = test_msgs::msg::Empty;
+  using MessageTAllocator = std::allocator<void>;
+  using MessageTDeallocator = rclcpp::allocator::Deleter<MessageTAllocator, MessageT>;
+  auto cb6 = [](MessageT::UniquePtrWithDeleter<MessageTDeallocator> unique_msg_ptr) -> void
+    {
+      (void) unique_msg_ptr;
+    };
+  static_assert(
+    rclcpp::subscription_traits::is_serialized_callback<decltype(cb6)>::value == false,
+    "passing a std::unique_ptr of test_msgs::msg::Empty is not a serialized callback");
+
+  auto cb7 = &not_serialized_unique_ptr_callback;
+  static_assert(
+    rclcpp::subscription_traits::is_serialized_callback<decltype(cb7)>::value == false,
+    "passing a fancy unique_ptr of test_msgs::msg::Empty is not a serialized callback");
+}
+
+TEST(TestSubscriptionTraits, callback_messages) {
+  static_assert(
+    std::is_same<
+      std::shared_ptr<char>,
+      rclcpp::function_traits::function_traits<
+        decltype(not_serialized_shared_ptr_callback)
+      >::template argument_type<0>
+    >::value, "wrong!");
+
+  static_assert(
+    std::is_same<
+      char,
+      rclcpp::subscription_traits::extract_message_type<
+        rclcpp::function_traits::function_traits<
+          decltype(not_serialized_shared_ptr_callback)
+        >::template argument_type<0>
+      >::type
+    >::value, "wrong!");
+
+  auto cb1 = &serialized_callback_copy;
+  static_assert(
+    std::is_same<
+      rcl_serialized_message_t,
+      rclcpp::subscription_traits::has_message_type<decltype(cb1)>::type>::value,
+    "serialized callback message type is rcl_serialized_message_t");
+
+  auto cb2 = &serialized_callback_shared_ptr;
+  static_assert(
+    std::is_same<
+      rcl_serialized_message_t,
+      rclcpp::subscription_traits::has_message_type<decltype(cb2)>::type>::value,
+    "serialized callback message type is rcl_serialized_message_t");
+
+  auto cb3 = &not_serialized_callback;
+  static_assert(
+    std::is_same<
+      char *,
+      rclcpp::subscription_traits::has_message_type<decltype(cb3)>::type>::value,
+    "not serialized callback message type is char");
+
+  auto cb4 = &not_serialized_shared_ptr_callback;
+  static_assert(
+    std::is_same<
+      char,
+      rclcpp::subscription_traits::has_message_type<decltype(cb4)>::type>::value,
+    "not serialized shared_ptr callback message type is std::shared_ptr<char>");
+
+  auto cb5 = [](rcl_serialized_message_t unused) -> void
+    {
+      (void) unused;
+    };
+  static_assert(
+    std::is_same<
+      rcl_serialized_message_t,
+      rclcpp::subscription_traits::has_message_type<decltype(cb5)>::type>::value,
+    "serialized callback message type is rcl_serialized_message_t");
+
+  using MessageT = test_msgs::msg::Empty;
+  using MessageTAllocator = std::allocator<MessageT>;
+  using MessageTDeallocator = rclcpp::allocator::Deleter<MessageTAllocator, MessageT>;
+  auto cb6 = [](std::unique_ptr<MessageT, MessageTDeallocator> unique_msg_ptr) -> void
+    {
+      (void) unique_msg_ptr;
+    };
+  static_assert(
+    std::is_same<
+      test_msgs::msg::Empty,
+      rclcpp::subscription_traits::has_message_type<decltype(cb6)>::type>::value,
+    "passing a std::unique_ptr of test_msgs::msg::Empty has message type Empty");
+
+  auto cb7 = &not_serialized_unique_ptr_callback;
+  static_assert(
+    std::is_same<
+      test_msgs::msg::Empty,
+      rclcpp::subscription_traits::has_message_type<decltype(cb7)>::type>::value,
+    "passing a fancy std::unique_ptr of test_msgs::msg::Empty has message type Empty");
+}

rclcpp/test/test_time.cpp

@@ -23,6 +23,18 @@
 #include "rclcpp/clock.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/time.hpp"
+#include "rclcpp/utilities.hpp"
+
+namespace
+{
+
+bool logical_eq(const bool a, const bool b)
+{
+  return (a && b) || ((!a) && !(b));
+}
+
+}  // namespace
+
 
 class TestTime : public ::testing::Test
 {
@@ -51,7 +63,7 @@ TEST(TestTime, time_sources) {
   EXPECT_NE(0, ros_now.sec);
   EXPECT_NE(0u, ros_now.nanosec);
 
-  rclcpp::Clock system_clock(RCL_ROS_TIME);
+  rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
   Time system_now = system_clock.now();
   EXPECT_NE(0, system_now.sec);
   EXPECT_NE(0u, system_now.nanosec);
@@ -63,7 +75,7 @@ TEST(TestTime, time_sources) {
 }
 
 TEST(TestTime, conversions) {
-  rclcpp::Clock system_clock(RCL_ROS_TIME);
+  rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
 
   rclcpp::Time now = system_clock.now();
   builtin_interfaces::msg::Time now_msg = now;
@@ -77,9 +89,9 @@ TEST(TestTime, conversions) {
 
   rclcpp::Time time = msg;
   EXPECT_EQ(
-    RCL_S_TO_NS(static_cast<uint64_t>(msg.sec)) + static_cast<uint64_t>(msg.nanosec),
+    RCL_S_TO_NS(static_cast<int64_t>(msg.sec)) + static_cast<int64_t>(msg.nanosec),
     time.nanoseconds());
-  EXPECT_EQ(static_cast<uint64_t>(msg.sec), RCL_NS_TO_S(time.nanoseconds()));
+  EXPECT_EQ(static_cast<int64_t>(msg.sec), RCL_NS_TO_S(time.nanoseconds()));
 
   builtin_interfaces::msg::Time negative_time_msg;
   negative_time_msg.sec = -1;
@@ -123,7 +135,7 @@ TEST(TestTime, operators) {
   EXPECT_ANY_THROW((void)(system_time > steady_time));
   EXPECT_ANY_THROW((void)(system_time - steady_time));
 
-  rclcpp::Clock system_clock(RCL_ROS_TIME);
+  rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
   rclcpp::Clock steady_clock(RCL_STEADY_TIME);
 
   rclcpp::Time now = system_clock.now();
@@ -148,11 +160,87 @@ TEST(TestTime, operators) {
   }
 }
 
+TEST(TestTime, overflow_detectors) {
+  /////////////////////////////////////////////////////////////////////////////
+  // Test logical_eq call first:
+  EXPECT_TRUE(logical_eq(false, false));
+  EXPECT_FALSE(logical_eq(false, true));
+  EXPECT_FALSE(logical_eq(true, false));
+  EXPECT_TRUE(logical_eq(true, true));
+
+  /////////////////////////////////////////////////////////////////////////////
+  // Exhaustive test of all int8_t values
+  using test_type_t = int8_t;
+  // big_type_t encompasses test_type_t:
+  //  big_type_t::min < test_type_t::min
+  //  big_type_t::max > test_type_t::max
+  using big_type_t = int16_t;
+  const big_type_t min_val = std::numeric_limits<test_type_t>::min();
+  const big_type_t max_val = std::numeric_limits<test_type_t>::max();
+  // 256 * 256 = 64K total loops, should be pretty fast on everything
+  for (big_type_t y = min_val; y <= max_val; ++y) {
+    for (big_type_t x = min_val; x <= max_val; ++x) {
+      const big_type_t sum = x + y;
+      const big_type_t diff = x - y;
+
+      const bool add_will_overflow =
+        rclcpp::add_will_overflow(test_type_t(x), test_type_t(y));
+      const bool add_did_overflow = sum > max_val;
+      EXPECT_TRUE(logical_eq(add_will_overflow, add_did_overflow));
+
+      const bool add_will_underflow =
+        rclcpp::add_will_underflow(test_type_t(x), test_type_t(y));
+      const bool add_did_underflow = sum < min_val;
+      EXPECT_TRUE(logical_eq(add_will_underflow, add_did_underflow));
+
+      const bool sub_will_overflow =
+        rclcpp::sub_will_overflow(test_type_t(x), test_type_t(y));
+      const bool sub_did_overflow = diff > max_val;
+      EXPECT_TRUE(logical_eq(sub_will_overflow, sub_did_overflow));
+
+      const bool sub_will_underflow =
+        rclcpp::sub_will_underflow(test_type_t(x), test_type_t(y));
+      const bool sub_did_underflow = diff < min_val;
+      EXPECT_TRUE(logical_eq(sub_will_underflow, sub_did_underflow));
+    }
+  }
+
+  // Few selected tests for int64_t
+  EXPECT_TRUE(rclcpp::add_will_overflow<int64_t>(INT64_MAX, 1));
+  EXPECT_FALSE(rclcpp::add_will_overflow<int64_t>(INT64_MAX, -1));
+  EXPECT_TRUE(rclcpp::add_will_underflow<int64_t>(INT64_MIN, -1));
+  EXPECT_FALSE(rclcpp::add_will_underflow<int64_t>(INT64_MIN, 1));
+
+  EXPECT_FALSE(rclcpp::sub_will_overflow<int64_t>(INT64_MAX, 1));
+  EXPECT_TRUE(rclcpp::sub_will_overflow<int64_t>(INT64_MAX, -1));
+  EXPECT_FALSE(rclcpp::sub_will_underflow<int64_t>(INT64_MIN, -1));
+  EXPECT_TRUE(rclcpp::sub_will_underflow<int64_t>(INT64_MIN, 1));
+}
+
 TEST(TestTime, overflows) {
   rclcpp::Time max_time(std::numeric_limits<rcl_time_point_value_t>::max());
   rclcpp::Time min_time(std::numeric_limits<rcl_time_point_value_t>::min());
   rclcpp::Duration one(1);
+  rclcpp::Duration two(2);
 
+  // Cross min/max
   EXPECT_THROW(max_time + one, std::overflow_error);
   EXPECT_THROW(min_time - one, std::underflow_error);
+  EXPECT_THROW(max_time - min_time, std::overflow_error);
+  EXPECT_THROW(min_time - max_time, std::underflow_error);
+  EXPECT_NO_THROW(max_time - max_time);
+  EXPECT_NO_THROW(min_time - min_time);
+
+  // Cross zero in both directions
+  rclcpp::Time one_time(1);
+  EXPECT_NO_THROW(one_time - two);
+
+  rclcpp::Time minus_one_time(-1);
+  EXPECT_NO_THROW(minus_one_time + two);
+
+  EXPECT_NO_THROW(one_time - minus_one_time);
+  EXPECT_NO_THROW(minus_one_time - one_time);
+
+  rclcpp::Time two_time(2);
+  EXPECT_NO_THROW(one_time - two_time);
 }

rclcpp/test/test_time_source.cpp

@@ -50,7 +50,6 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
-
 TEST_F(TestTimeSource, detachUnattached) {
   rclcpp::TimeSource ts;
 
@@ -93,18 +92,17 @@ TEST_F(TestTimeSource, clock) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
+  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
       rmw_qos_profile_default);
   rclcpp::WallRate loop_rate(50);
   for (int i = 0; i < 5; ++i) {
     if (!rclcpp::ok()) {
       break;  // Break for ctrl-c
     }
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
-    msg->sec = i;
-    msg->nanosec = 1000;
+    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
+    msg->clock.sec = i;
+    msg->clock.nanosec = 1000;
     clock_pub->publish(msg);
-    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
     rclcpp::spin_some(node);
     loop_rate.sleep();
   }
@@ -116,7 +114,7 @@ TEST_F(TestTimeSource, clock) {
 
   auto t_out = ros_clock->now();
 
-  EXPECT_NE(0UL, t_out.nanoseconds());
+  EXPECT_NE(0L, t_out.nanoseconds());
   EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
   EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
 }
@@ -163,7 +161,7 @@ TEST_F(TestTimeSource, callbacks) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
+  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
       rmw_qos_profile_default);
 
   rclcpp::WallRate loop_rate(50);
@@ -171,11 +169,10 @@ TEST_F(TestTimeSource, callbacks) {
     if (!rclcpp::ok()) {
       break;  // Break for ctrl-c
     }
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
-    msg->sec = i;
-    msg->nanosec = 1000;
+    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
+    msg->clock.sec = i;
+    msg->clock.nanosec = 1000;
     clock_pub->publish(msg);
-    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
     rclcpp::spin_some(node);
     loop_rate.sleep();
   }
@@ -190,7 +187,7 @@ TEST_F(TestTimeSource, callbacks) {
 
   auto t_out = ros_clock->now();
 
-  EXPECT_NE(0UL, t_out.nanoseconds());
+  EXPECT_NE(0L, t_out.nanoseconds());
   EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
   EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
 
@@ -205,11 +202,10 @@ TEST_F(TestTimeSource, callbacks) {
     if (!rclcpp::ok()) {
       break;  // Break for ctrl-c
     }
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
-    msg->sec = i;
-    msg->nanosec = 2000;
+    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
+    msg->clock.sec = i;
+    msg->clock.nanosec = 2000;
     clock_pub->publish(msg);
-    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
     rclcpp::spin_some(node);
     loop_rate.sleep();
   }
@@ -222,7 +218,7 @@ TEST_F(TestTimeSource, callbacks) {
 
   t_out = ros_clock->now();
 
-  EXPECT_NE(0UL, t_out.nanoseconds());
+  EXPECT_NE(0L, t_out.nanoseconds());
   EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
   EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
 }
@@ -230,7 +226,7 @@ TEST_F(TestTimeSource, callbacks) {
 void trigger_clock_changes(
   rclcpp::Node::SharedPtr node)
 {
-  auto clock_pub = node->create_publisher<builtin_interfaces::msg::Time>("clock",
+  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
       rmw_qos_profile_default);
 
   rclcpp::executors::SingleThreadedExecutor executor;
@@ -241,11 +237,10 @@ void trigger_clock_changes(
     if (!rclcpp::ok()) {
       break;  // Break for ctrl-c
     }
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
-    msg->sec = i;
-    msg->nanosec = 1000;
+    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
+    msg->clock.sec = i;
+    msg->clock.nanosec = 1000;
     clock_pub->publish(msg);
-    // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl;
     executor.spin_once(1000000ns);
     loop_rate.sleep();
   }
@@ -296,7 +291,7 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
 
   auto t_out = ros_clock->now();
 
-  EXPECT_NE(0UL, t_out.nanoseconds());
+  EXPECT_NE(0L, t_out.nanoseconds());
   EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
   EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
 
@@ -320,7 +315,7 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
 
   t_out = ros_clock->now();
 
-  EXPECT_NE(0UL, t_out.nanoseconds());
+  EXPECT_NE(0L, t_out.nanoseconds());
   EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
   EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
 }
@@ -334,13 +329,24 @@ TEST_F(TestTimeSource, parameter_activation) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  auto parameter_service = std::make_shared<rclcpp::ParameterService>(node);
   auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
 
   using namespace std::chrono_literals;
   EXPECT_TRUE(parameters_client->wait_for_service(2s));
   auto set_parameters_results = parameters_client->set_parameters({
-    rclcpp::parameter::ParameterVariant("use_sim_time", true)
+    rclcpp::Parameter("use_sim_time", true)
+  });
+  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);
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  set_parameters_results = parameters_client->set_parameters({
+    rclcpp::Parameter("use_sim_time", rclcpp::ParameterType::PARAMETER_NOT_SET)
   });
   for (auto & result : set_parameters_results) {
     EXPECT_TRUE(result.successful);
@@ -348,28 +354,21 @@ TEST_F(TestTimeSource, parameter_activation) {
   rclcpp::spin_some(node);
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
-
   set_parameters_results = parameters_client->set_parameters({
-    rclcpp::parameter::ParameterVariant("use_sim_time", rclcpp::parameter::PARAMETER_NOT_SET)
-  });
-  for (auto & result : set_parameters_results) {
-    EXPECT_TRUE(result.successful);
-  }
-  EXPECT_TRUE(ros_clock->ros_time_is_active());
-
-  set_parameters_results = parameters_client->set_parameters({
-    rclcpp::parameter::ParameterVariant("use_sim_time", false)
+    rclcpp::Parameter("use_sim_time", false)
   });
   for (auto & result : set_parameters_results) {
     EXPECT_TRUE(result.successful);
   }
+  rclcpp::spin_some(node);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
   set_parameters_results = parameters_client->set_parameters({
-    rclcpp::parameter::ParameterVariant("use_sim_time", rclcpp::parameter::PARAMETER_NOT_SET)
+    rclcpp::Parameter("use_sim_time", rclcpp::ParameterType::PARAMETER_NOT_SET)
   });
   for (auto & result : set_parameters_results) {
     EXPECT_TRUE(result.successful);
   }
+  rclcpp::spin_some(node);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 }

rclcpp/test/test_utilities.cpp

@@ -0,0 +1,51 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/utilities.hpp"
+
+TEST(TestUtilities, remove_ros_arguments) {
+  const char * const argv[] = {"process_name", "-d", "__ns:=/foo/bar",
+    "__ns:=/fiz/buz", "--foo=bar", "--baz"};
+  int argc = sizeof(argv) / sizeof(const char *);
+  auto args = rclcpp::remove_ros_arguments(argc, argv);
+
+  ASSERT_EQ(4u, args.size());
+  ASSERT_EQ(std::string{"process_name"}, args[0]);
+  ASSERT_EQ(std::string{"-d"}, args[1]);
+  ASSERT_EQ(std::string{"--foo=bar"}, args[2]);
+  ASSERT_EQ(std::string{"--baz"}, args[3]);
+}
+
+TEST(TestUtilities, remove_ros_arguments_null) {
+  // In the case of a C executable, we would expect to get
+  // argc=1 and argv = ["process_name"], so this is an invalid input.
+  ASSERT_THROW({
+    rclcpp::remove_ros_arguments(0, nullptr);
+  }, rclcpp::exceptions::RCLErrorBase);
+}
+
+TEST(TestUtilities, init_with_args) {
+  const char * const argv[] = {"process_name"};
+  int argc = sizeof(argv) / sizeof(const char *);
+  auto other_args = rclcpp::init_and_remove_ros_arguments(argc, argv);
+
+  ASSERT_EQ(1u, other_args.size());
+  ASSERT_EQ(std::string{"process_name"}, other_args[0]);
+}

rclcpp_lifecycle/CHANGELOG.rst

@@ -0,0 +1,15 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rclcpp_lifecycle
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.5.0 (2018-06-25)
+------------------
+* Added functions that allow you to publish serialized messages and received serialized messages in your subscription callback. (`#388 <https://github.com/ros2/rclcpp/issues/388>`_)
+* Added ability to initialize parameter values in a node with an argument to the Node constructor. (`#486 <https://github.com/ros2/rclcpp/issues/486>`_)
+* Nodes now autostart the ROS parameter services which let you get, set, and list parameters in a node. (`#478 <https://github.com/ros2/rclcpp/issues/478>`_)
+* Fixed a bug that occurred when mixing ``std::shared_ptr`` and ``std::bind``. (`#470 <https://github.com/ros2/rclcpp/issues/470>`_)
+* Added ability to pass command line arguments to the Node constructor. (`#461 <https://github.com/ros2/rclcpp/issues/461>`_)
+* Changed library export order for static linking. (`#446 <https://github.com/ros2/rclcpp/issues/446>`_)
+* Now depends on ``ament_cmake_ros``. (`#444 <https://github.com/ros2/rclcpp/issues/444>`_)
+* Updaed code to use logging macros rather than ``fprintf()``. (`#439 <https://github.com/ros2/rclcpp/issues/439>`_)
+* Contributors: Dirk Thomas, Guillaume Autran, Karsten Knese, Michael Carroll, Mikael Arguedas, Shane Loretz, dhood

rclcpp_lifecycle/CMakeLists.txt

@@ -10,7 +10,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcl_lifecycle REQUIRED)
 find_package(std_msgs REQUIRED)
@@ -20,16 +20,16 @@ include_directories(include)
 
 ### CPP High level library
 add_library(rclcpp_lifecycle
-  SHARED
   src/lifecycle_node.cpp
   src/node_interfaces/lifecycle_node_interface.cpp
   src/state.cpp
   src/transition.cpp
 )
+# specific order: dependents before dependencies
 ament_target_dependencies(rclcpp_lifecycle
-  "lifecycle_msgs"
+  "rclcpp"
   "rcl_lifecycle"
-  "rclcpp")
+  "lifecycle_msgs")
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
@@ -101,11 +101,12 @@ if(BUILD_TESTING)
   endif()
 endif()
 
+# specific order: dependents before dependencies
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(rcl_lifecycle)
 ament_export_dependencies(lifecycle_msgs)
-ament_export_include_directories(include)
-ament_export_libraries(${PROJECT_NAME})
 ament_package()
 
 install(DIRECTORY include/

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -88,6 +88,10 @@ public:
    * \param[in] node_name Name of the node.
    * \param[in] node_name Namespace of the node.
    * \param[in] context The context for the node (usually represents the state of a process).
+   * \param[in] arguments Command line arguments that should apply only to this node.
+   * \param[in] initial_parameters a list of initial values for parameters on the node.
+   * This can be used to provide remapping rules that only affect one instance.
+   * \param[in] use_global_arguments False to prevent node using arguments passed to the process.
    * \param[in] use_intra_process_comms True to use the optimized intra-process communication
    * pipeline to pass messages between nodes in the same process using shared memory.
    */
@@ -96,7 +100,11 @@ public:
     const std::string & node_name,
     const std::string & namespace_,
     rclcpp::Context::SharedPtr context,
-    bool use_intra_process_comms = false);
+    const std::vector<std::string> & arguments,
+    const std::vector<rclcpp::Parameter> & initial_parameters,
+    bool use_global_arguments = true,
+    bool use_intra_process_comms = false,
+    bool start_parameter_services = true);
 
   RCLCPP_LIFECYCLE_PUBLIC
   virtual ~LifecycleNode();
@@ -180,7 +188,8 @@ public:
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
     bool ignore_local_publications = false,
-    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<
+      typename rclcpp::subscription_traits::has_message_type<CallbackT>::type, Alloc>::SharedPtr
     msg_mem_strat = nullptr,
     std::shared_ptr<Alloc> allocator = nullptr);
 
@@ -246,25 +255,25 @@ public:
 
   RCLCPP_LIFECYCLE_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_LIFECYCLE_PUBLIC
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
 
   RCLCPP_LIFECYCLE_PUBLIC
-  std::vector<rclcpp::parameter::ParameterVariant>
+  std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & names) const;
 
   RCLCPP_LIFECYCLE_PUBLIC
-  rclcpp::parameter::ParameterVariant
+  rclcpp::Parameter
   get_parameter(const std::string & name) const;
 
   RCLCPP_LIFECYCLE_PUBLIC
   bool
   get_parameter(
     const std::string & name,
-    rclcpp::parameter::ParameterVariant & parameter) const;
+    rclcpp::Parameter & parameter) const;
 
   template<typename ParameterT>
   bool

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp

@@ -23,6 +23,7 @@
 #include "rclcpp/intra_process_manager.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/create_publisher.hpp"
+#include "rclcpp/create_service.hpp"
 #include "rclcpp/create_subscription.hpp"
 #include "rclcpp/type_support_decl.hpp"
 
@@ -75,22 +76,23 @@ LifecycleNode::create_subscription(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group,
   bool ignore_local_publications,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  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)
 {
+  using CallbackMessageT = typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
+
   if (!allocator) {
     allocator = std::make_shared<Alloc>();
   }
 
   if (!msg_mem_strat) {
     using rclcpp::message_memory_strategy::MessageMemoryStrategy;
-    msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
+    msg_mem_strat = MessageMemoryStrategy<CallbackMessageT, Alloc>::create_default();
   }
 
-  return rclcpp::create_subscription<
-    MessageT, CallbackT, Alloc,
-    rclcpp::Subscription<MessageT, Alloc>>(
+  return rclcpp::create_subscription<MessageT, CallbackT, Alloc, CallbackMessageT, SubscriptionT>(
     this->node_topics_.get(),
     topic_name,
     std::forward<CallbackT>(callback),
@@ -176,27 +178,18 @@ LifecycleNode::create_service(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
-  any_service_callback.set(std::forward<CallbackT>(callback));
-
-  rcl_service_options_t service_options = rcl_service_get_default_options();
-  service_options.qos = qos_profile;
-
-  auto serv = rclcpp::Service<ServiceT>::make_shared(
-    node_base_->get_shared_rcl_node_handle(),
-    service_name, any_service_callback, service_options);
-  auto serv_base_ptr = std::dynamic_pointer_cast<rclcpp::ServiceBase>(serv);
-  node_services_->add_service(serv_base_ptr, group);
-  return serv;
+  return rclcpp::create_service<ServiceT, CallbackT>(
+    node_base_, node_services_,
+    service_name, std::forward<CallbackT>(callback), qos_profile, group);
 }
 
 template<typename ParameterT>
 bool
 LifecycleNode::get_parameter(const std::string & name, ParameterT & parameter) const
 {
-  rclcpp::parameter::ParameterVariant parameter_variant(name, parameter);
-  bool result = get_parameter(name, parameter_variant);
-  parameter = parameter_variant.get_value<ParameterT>();
+  rclcpp::Parameter param(name, parameter);
+  bool result = get_parameter(name, param);
+  parameter = param.get_value<ParameterT>();
 
   return result;
 }

rclcpp_lifecycle/package.xml

@@ -2,12 +2,12 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_lifecycle</name>
-  <version>0.4.0</version>
+  <version>0.5.0</version>
   <description>Package containing a prototype for lifecycle implementation</description>
   <maintainer email="karsten@osrfoundation.org">Karsten Knese</maintainer>
   <license>Apache License 2.0</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <build_depend>rclcpp</build_depend>

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -35,6 +35,7 @@
 #include "rclcpp/node_interfaces/node_services.hpp"
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
+#include "rclcpp/parameter_service.hpp"
 
 #include "lifecycle_node_interface_impl.hpp"  // implementation
 
@@ -49,23 +50,36 @@ LifecycleNode::LifecycleNode(
     node_name,
     namespace_,
     rclcpp::contexts::default_context::get_global_default_context(),
-    use_intra_process_comms)
+    {},
+    {},
+    true,
+    use_intra_process_comms,
+    true)
 {}
 
 LifecycleNode::LifecycleNode(
   const std::string & node_name,
   const std::string & namespace_,
   rclcpp::Context::SharedPtr context,
-  bool use_intra_process_comms)
-: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, 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)
+: node_base_(new rclcpp::node_interfaces::NodeBase(
+      node_name, namespace_, context, arguments, use_global_arguments)),
   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())),
   node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
   node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
   node_parameters_(new rclcpp::node_interfaces::NodeParameters(
-      node_topics_.get(),
-      use_intra_process_comms
+      node_base_,
+      node_topics_,
+      node_services_,
+      initial_parameters,
+      use_intra_process_comms,
+      start_parameter_services
     )),
   node_clock_(new rclcpp::node_interfaces::NodeClock(
       node_base_,
@@ -126,26 +140,26 @@ LifecycleNode::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr gr
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
 LifecycleNode::set_parameters(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   return node_parameters_->set_parameters(parameters);
 }
 
 rcl_interfaces::msg::SetParametersResult
 LifecycleNode::set_parameters_atomically(
-  const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters)
 {
   return node_parameters_->set_parameters_atomically(parameters);
 }
 
-std::vector<rclcpp::parameter::ParameterVariant>
+std::vector<rclcpp::Parameter>
 LifecycleNode::get_parameters(
   const std::vector<std::string> & names) const
 {
   return node_parameters_->get_parameters(names);
 }
 
-rclcpp::parameter::ParameterVariant
+rclcpp::Parameter
 LifecycleNode::get_parameter(const std::string & name) const
 {
   return node_parameters_->get_parameter(name);
@@ -153,7 +167,7 @@ LifecycleNode::get_parameter(const std::string & name) const
 
 bool LifecycleNode::get_parameter(
   const std::string & name,
-  rclcpp::parameter::ParameterVariant & parameter) const
+  rclcpp::Parameter & parameter) const
 {
   return node_parameters_->get_parameter(name, parameter);
 }

rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp

@@ -68,7 +68,9 @@ public:
     auto ret = rcl_lifecycle_state_machine_fini(
       &state_machine_, node_handle, &node_options->allocator);
     if (ret != RCL_RET_OK) {
-      fprintf(stderr, "FATAL: failed to destroy rcl_state_machine\n");
+      RCUTILS_LOG_FATAL_NAMED(
+        "rclcpp_lifecycle",
+        "failed to destroy rcl_state_machine");
     }
   }
 
@@ -295,11 +297,14 @@ public:
       return RCL_RET_ERROR;
     }
 
+    constexpr bool publish_update = true;
     // keep the initial state to pass to a transition callback
     State initial_state(state_machine_.current_state);
 
     uint8_t transition_id = lifecycle_transition;
-    if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, true) != RCL_RET_OK) {
+    if (rcl_lifecycle_trigger_transition(
+        &state_machine_, transition_id, publish_update) != RCL_RET_OK)
+    {
       RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s",
         transition_id, state_machine_.current_state->label, rcl_get_error_string_safe())
       return RCL_RET_ERROR;
@@ -309,7 +314,7 @@ public:
       state_machine_.current_state->id, initial_state);
 
     if (rcl_lifecycle_trigger_transition(
-        &state_machine_, cb_return_code, true) != RCL_RET_OK)
+        &state_machine_, cb_return_code, publish_update) != RCL_RET_OK)
     {
       RCUTILS_LOG_ERROR("Failed to finish transition %u. Current state is now: %s",
         transition_id, state_machine_.current_state->label)
@@ -324,13 +329,17 @@ public:
         state_machine_.current_state->id, initial_state);
       if (error_resolved == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) {
         // We call cleanup on the error state
-        if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) {
+        if (rcl_lifecycle_trigger_transition(
+            &state_machine_, error_resolved, publish_update) != RCL_RET_OK)
+        {
           RCUTILS_LOG_ERROR("Failed to call cleanup on error state")
           return RCL_RET_ERROR;
         }
       } else {
         // We call shutdown on the error state
-        if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) {
+        if (rcl_lifecycle_trigger_transition(
+            &state_machine_, error_resolved, publish_update) != RCL_RET_OK)
+        {
           RCUTILS_LOG_ERROR("Failed to call cleanup on error state")
           return RCL_RET_ERROR;
         }
@@ -357,9 +366,9 @@ public:
       } catch (const std::exception &) {
         // TODO(karsten1987): Windows CI doens't let me print the msg here
         // the todo is to forward the exception to the on_error callback
-        // fprintf(stderr, "Caught exception in callback for transition %d\n",
-        //  it->first);
-        // fprintf(stderr, "Original error msg: %s\n", e.what());
+        // RCUTILS_LOG_ERROR("Caught exception in callback for transition %d\n",
+        //  it->first)
+        // RCUTILS_LOG_ERROR("Original error msg: %s\n", e.what())
         // maybe directly go for error handling here
         // and pass exception along with it
         cb_success = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
@@ -407,9 +416,9 @@ public:
   using ChangeStateSrvPtr = std::shared_ptr<rclcpp::Service<ChangeStateSrv>>;
   using GetStateSrvPtr = std::shared_ptr<rclcpp::Service<GetStateSrv>>;
   using GetAvailableStatesSrvPtr =
-      std::shared_ptr<rclcpp::Service<GetAvailableStatesSrv>>;
+    std::shared_ptr<rclcpp::Service<GetAvailableStatesSrv>>;
   using GetAvailableTransitionsSrvPtr =
-      std::shared_ptr<rclcpp::Service<GetAvailableTransitionsSrv>>;
+    std::shared_ptr<rclcpp::Service<GetAvailableTransitionsSrv>>;
 
   NodeBasePtr node_base_interface_;
   NodeServicesPtr node_services_interface_;

rclcpp_lifecycle/test/test_lifecycle_node.cpp

@@ -203,3 +203,12 @@ TEST_F(TestDefaultStateMachine, bad_mood) {
   // check if all callbacks were successfully overwritten
   EXPECT_EQ(static_cast<size_t>(1), test_node->number_of_callbacks);
 }
+
+TEST_F(TestDefaultStateMachine, lifecycle_subscriber) {
+  auto test_node = std::make_shared<MoodyLifecycleNode<GoodMood>>("testnode");
+
+  auto cb = [](const std::shared_ptr<lifecycle_msgs::msg::State> msg) {(void) msg;};
+  auto lifecycle_sub = test_node->create_subscription<lifecycle_msgs::msg::State>("~/empty", cb);
+
+  SUCCEED();
+}

rclcpp_lifecycle/test/test_register_custom_callbacks.cpp

@@ -137,16 +137,16 @@ public:
 TEST_F(TestRegisterCustomCallbacks, custom_callbacks) {
   auto test_node = std::make_shared<CustomLifecycleNode>("testnode");
 
-  test_node->register_on_configure(
-    std::bind(&CustomLifecycleNode::on_custom_configure, test_node, std::placeholders::_1));
-  test_node->register_on_cleanup(std::bind(&CustomLifecycleNode::on_custom_cleanup, test_node,
-    std::placeholders::_1));
-  test_node->register_on_shutdown(std::bind(&CustomLifecycleNode::on_custom_shutdown, test_node,
-    std::placeholders::_1));
-  test_node->register_on_activate(std::bind(&CustomLifecycleNode::on_custom_activate, test_node,
-    std::placeholders::_1));
+  test_node->register_on_configure(std::bind(&CustomLifecycleNode::on_custom_configure,
+    test_node.get(), std::placeholders::_1));
+  test_node->register_on_cleanup(std::bind(&CustomLifecycleNode::on_custom_cleanup,
+    test_node.get(), std::placeholders::_1));
+  test_node->register_on_shutdown(std::bind(&CustomLifecycleNode::on_custom_shutdown,
+    test_node.get(), std::placeholders::_1));
+  test_node->register_on_activate(std::bind(&CustomLifecycleNode::on_custom_activate,
+    test_node.get(), std::placeholders::_1));
   test_node->register_on_deactivate(std::bind(&CustomLifecycleNode::on_custom_deactivate,
-    test_node, std::placeholders::_1));
+    test_node.get(), std::placeholders::_1));
 
   EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
   EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition(