0.0.3

* parameter client takes node interfaces

* correct wrong copy paste

* correctly fetch node name

* use node_topics_interface for creating parameter event

* fix typos

.github/ISSUE_TEMPLATE.md

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

rclcpp/.gitignore

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

rclcpp/CMakeLists.txt

@@ -3,17 +3,26 @@ cmake_minimum_required(VERSION 3.5)
 project(rclcpp)
 
 find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
-find_package(rmw_implementation_cmake REQUIRED)
 find_package(rosidl_generator_cpp REQUIRED)
+find_package(rosidl_typesupport_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
 
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-std=c++14")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  # we dont use add_compile_options with pedantic in message packages
+  # because the Python C extensions dont comply with it
+  # TODO(mikaelarguedas) change to add_compile_options
+  # once this is not a message package anymore
+  # https://github.com/ros2/system_tests/issues/191
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
 endif()
 
 include_directories(include)
@@ -28,6 +37,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/exceptions.cpp
   src/rclcpp/executor.cpp
   src/rclcpp/executors.cpp
+  src/rclcpp/expand_topic_or_service_name.cpp
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
   src/rclcpp/graph_listener.cpp
@@ -48,40 +58,37 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/publisher.cpp
   src/rclcpp/service.cpp
   src/rclcpp/subscription.cpp
+  src/rclcpp/time.cpp
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
   src/rclcpp/utilities.cpp
 )
 
-macro(target)
-  if(NOT target_suffix STREQUAL "")
-    get_rcl_information("${rmw_implementation}" "rcl${target_suffix}")
-  endif()
-  add_library(${PROJECT_NAME}${target_suffix} SHARED
-    ${${PROJECT_NAME}_SRCS})
-  ament_target_dependencies(${PROJECT_NAME}${target_suffix}
-    "rcl${target_suffix}"
-    "rosidl_generator_cpp"
-    "rosidl_typesupport_cpp")
+add_library(${PROJECT_NAME} SHARED
+  ${${PROJECT_NAME}_SRCS})
+ament_target_dependencies(${PROJECT_NAME}
+  "builtin_interfaces"
+  "rcl"
+  "rosidl_generator_cpp"
+  "rosidl_typesupport_cpp")
 
-  # Causes the visibility macros to use dllexport rather than dllimport,
-  # which is appropriate when building the dll but not consuming it.
-  target_compile_definitions(${PROJECT_NAME}${target_suffix}
-    PRIVATE "RCLCPP_BUILDING_LIBRARY")
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME}
+  PRIVATE "RCLCPP_BUILDING_LIBRARY")
 
-  install(
-    TARGETS ${PROJECT_NAME}${target_suffix}
-    ARCHIVE DESTINATION lib
-    LIBRARY DESTINATION lib
-    RUNTIME DESTINATION bin
-  )
-endmacro()
-
-call_for_each_rmw_implementation(target GENERATE_DEFAULT)
+install(
+  TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  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)
 
 ament_export_include_directories(include)
@@ -92,6 +99,28 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 
+  find_package(rmw_implementation_cmake REQUIRED)
+
+  ament_add_gtest(test_client test/test_client.cpp)
+  if(TARGET test_client)
+    target_include_directories(test_client PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_client ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp)
+  if(TARGET test_expand_topic_or_service_name)
+    target_include_directories(test_expand_topic_or_service_name PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME})
+  endif()
   ament_add_gtest(test_function_traits test/test_function_traits.cpp)
   if(TARGET test_function_traits)
     target_include_directories(test_function_traits PUBLIC
@@ -121,6 +150,26 @@ if(BUILD_TESTING)
       ${rosidl_typesupport_cpp_INCLUDE_DIRS}
     )
   endif()
+  ament_add_gtest(test_node test/test_node.cpp)
+  if(TARGET test_node)
+    target_include_directories(test_node PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_node ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_publisher test/test_publisher.cpp)
+  if(TARGET test_publisher)
+    target_include_directories(test_publisher PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_publisher ${PROJECT_NAME})
+  endif()
   ament_add_gtest(test_rate test/test_rate.cpp
     ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
   if(TARGET test_rate)
@@ -134,6 +183,26 @@ if(BUILD_TESTING)
       ${PROJECT_NAME}
     )
   endif()
+  ament_add_gtest(test_service test/test_service.cpp)
+  if(TARGET test_service)
+    target_include_directories(test_service PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_service ${PROJECT_NAME})
+  endif()
+  ament_add_gtest(test_subscription test/test_subscription.cpp)
+  if(TARGET test_subscription)
+    target_include_directories(test_subscription PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_subscription ${PROJECT_NAME})
+  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
@@ -155,7 +224,7 @@ if(BUILD_TESTING)
 
   set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
   if(WIN32)
-    set(append_library_dirs "${append_library_dirs}/$<CONFIGURATION>")
+    set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
   endif()
 
   ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp
@@ -174,6 +243,14 @@ if(BUILD_TESTING)
         mock_msgs ${typesupport_impl_c})
     endforeach()
   endif()
+
+  ament_add_gtest(test_time test/test_time.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_time)
+    ament_target_dependencies(test_time
+      "rcl")
+    target_link_libraries(test_time ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_package(

rclcpp/Doxyfile

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

rclcpp/cmake/get_rclcpp_information.cmake

@@ -1,93 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-#
-# Get all information about rclcpp for a specific RMW implementation.
-#
-# It sets the common variables _DEFINITIONS, _INCLUDE_DIRS and _LIBRARIES
-# with the given prefix.
-#
-# :param rmw_implementation: the RMW implementation name
-# :type rmw_implementation: string
-# :param var_prefix: the prefix of all output variable names
-# :type var_prefix: string
-#
-macro(get_rclcpp_information rmw_implementation var_prefix)
-  # pretend to be a "package"
-  # so that the variables can be used by various functions / macros
-  set(${var_prefix}_FOUND TRUE)
-
-  # Get rcl using the existing macro
-  if(NOT target_suffix STREQUAL "")
-    get_rcl_information("${rmw_implementation}" "rcl${target_suffix}")
-  endif()
-
-  # include directories
-  normalize_path(${var_prefix}_INCLUDE_DIRS
-    "${rclcpp_DIR}/../../../include")
-
-  # libraries
-  set(_libs)
-  # search for library relative to this CMake file
-  set(_library_target "rclcpp")
-  get_available_rmw_implementations(_rmw_impls)
-  list(LENGTH _rmw_impls _rmw_impls_length)
-  if(_rmw_impls_length GREATER 1)
-    set(_library_target "${_library_target}__${rmw_implementation}")
-  endif()
-  set(_lib "NOTFOUND")
-  find_library(
-    _lib NAMES "${_library_target}"
-    PATHS "${rclcpp_DIR}/../../../lib"
-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
-  )
-  if(NOT _lib)
-    # warn about not existing library and ignore it
-    message(WARNING "Package 'rclcpp' doesn't contain the library '${_library_target}'")
-  elseif(NOT IS_ABSOLUTE "${_lib}")
-    # the found library must be an absolute path
-    message(FATAL_ERROR "Package 'rclcpp' found the library '${_library_target}' at '${_lib}' which is not an absolute path")
-  elseif(NOT EXISTS "${_lib}")
-    # the found library must exist
-    message(FATAL_ERROR "Package 'rclcpp' found the library '${_lib}' which doesn't exist")
-  else()
-    list(APPEND _libs "${_lib}")
-  endif()
-
-  # dependencies
-  set(_exported_dependencies
-    "rcl_interfaces"
-    "rcl${target_suffix}"
-    "rosidl_generator_cpp"
-    "rosidl_typesupport_cpp")
-  set(${var_prefix}_DEFINITIONS)
-  foreach(_dep ${_exported_dependencies})
-    if(NOT ${_dep}_FOUND)
-      find_package("${_dep}" QUIET REQUIRED)
-    endif()
-    if(${_dep}_DEFINITIONS)
-      list_append_unique(${var_prefix}_DEFINITIONS "${${_dep}_DEFINITIONS}")
-    endif()
-    if(${_dep}_INCLUDE_DIRS)
-      list_append_unique(${var_prefix}_INCLUDE_DIRS "${${_dep}_INCLUDE_DIRS}")
-    endif()
-    if(${_dep}_LIBRARIES)
-      list(APPEND _libs "${${_dep}_LIBRARIES}")
-    endif()
-  endforeach()
-  if(_libs)
-    ament_libraries_deduplicate(_libs "${_libs}")
-  endif()
-  set(${var_prefix}_LIBRARIES "${_libs}")
-endmacro()

rclcpp/include/rclcpp/client.hpp

@@ -27,11 +27,13 @@
 #include "rcl/error_handling.h"
 #include "rcl/wait.h"
 
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 #include "rmw/error_handling.h"
@@ -66,6 +68,10 @@ public:
   const std::string &
   get_service_name() const;
 
+  RCLCPP_PUBLIC
+  rcl_client_t *
+  get_client_handle();
+
   RCLCPP_PUBLIC
   const rcl_client_t *
   get_client_handle() const;
@@ -98,6 +104,10 @@ protected:
 
   RCLCPP_PUBLIC
   rcl_node_t *
+  get_rcl_node_handle();
+
+  RCLCPP_PUBLIC
+  const rcl_node_t *
   get_rcl_node_handle() const;
 
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
@@ -138,14 +148,24 @@ public:
     using rosidl_typesupport_cpp::get_service_type_support_handle;
     auto service_type_support_handle =
       get_service_type_support_handle<ServiceT>();
-    if (rcl_client_init(&client_handle_, this->get_rcl_node_handle(),
-      service_type_support_handle, service_name.c_str(), &client_options) != RCL_RET_OK)
-    {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create client: ") +
-        rcl_get_error_string_safe());
-      // *INDENT-ON*
+    rcl_ret_t ret = rcl_client_init(
+      &client_handle_,
+      this->get_rcl_node_handle(),
+      service_type_support_handle,
+      service_name.c_str(),
+      &client_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+        auto rcl_node_handle = this->get_rcl_node_handle();
+        // this will throw on any validation problem
+        rcl_reset_error();
+        expand_topic_or_service_name(
+          service_name,
+          rcl_node_get_name(rcl_node_handle),
+          rcl_node_get_namespace(rcl_node_handle),
+          true);
+      }
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
     }
   }
 
@@ -153,7 +173,8 @@ public:
   {
     if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) {
       fprintf(stderr,
-        "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
+        "Error in destruction of rcl client handle: %s\n", rcl_get_error_string_safe());
+      rcl_reset_error();
     }
   }
 
@@ -212,11 +233,9 @@ public:
   {
     std::lock_guard<std::mutex> lock(pending_requests_mutex_);
     int64_t sequence_number;
-    if (RCL_RET_OK != rcl_send_request(get_client_handle(), request.get(), &sequence_number)) {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to send request: ") + rcl_get_error_string_safe());
-      // *INDENT-ON*
+    rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
     }
 
     SharedPromise call_promise = std::make_shared<Promise>();

rclcpp/include/rclcpp/exceptions.hpp

@@ -35,18 +35,88 @@ public:
   : std::runtime_error("node is invalid") {}
 };
 
+/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid.
+class NameValidationError : public std::invalid_argument
+{
+public:
+  NameValidationError(
+    const char * name_type_,
+    const char * name_,
+    const char * error_msg_,
+    size_t invalid_index_)
+  : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)),
+    name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_)
+  {}
+
+  static std::string
+  format_error(
+    const char * name_type,
+    const char * name,
+    const char * error_msg,
+    size_t invalid_index);
+
+  const std::string name_type;
+  const std::string name;
+  const std::string error_msg;
+  const size_t invalid_index;
+};
+
+/// Thrown when a node name is invalid.
+class InvalidNodeNameError : public NameValidationError
+{
+public:
+  InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index)
+  : NameValidationError("node name", node_name, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a node namespace is invalid.
+class InvalidNamespaceError : public NameValidationError
+{
+public:
+  InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("namespace", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a topic name is invalid.
+class InvalidTopicNameError : public NameValidationError
+{
+public:
+  InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("topic name", namespace_, error_msg, invalid_index)
+  {}
+};
+
+/// Thrown when a service name is invalid.
+class InvalidServiceNameError : public NameValidationError
+{
+public:
+  InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index)
+  : NameValidationError("service name", namespace_, error_msg, invalid_index)
+  {}
+};
+
 /// Throw a C++ std::exception which was created based on an rcl error.
-/*
+/**
+ * Passing nullptr for reset_error is safe and will avoid calling any function
+ * to reset the error.
+ *
  * \param ret the return code for the current error state
  * \param prefix string to prefix to the error if applicable (not all errors have custom messages)
- * \param reset_error if true rcl_reset_error() is called before returning
+ * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used
+ * \param reset_error function to be called before throwing which whill clear the error state
  * \throws std::invalid_argument if ret is RCL_RET_OK
  * \throws std::runtime_error if the rcl_get_error_state returns 0
  * \throws RCLErrorBase some child class exception based on ret
  */
 RCLCPP_PUBLIC
 void
-throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix = "", bool reset_error = true);
+throw_from_rcl_error(
+  rcl_ret_t ret,
+  const std::string & prefix = "",
+  const rcl_error_state_t * error_state = nullptr,
+  void (* reset_error)() = rcl_reset_error);
 
 class RCLErrorBase
 {

rclcpp/include/rclcpp/executor.hpp

@@ -202,14 +202,13 @@ public:
 
   /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
   /**
-   * \param[in] executor The executor which will spin the node.
-   * \param[in] node_ptr The node to spin.
    * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this
-     function.
+   *   function.
    * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
-     -1 is block forever, 0 is non-blocking.
-     If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code.
-   * \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT.
+   *   `-1` is block forever, `0` is non-blocking.
+   *   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return
+   *   code.
+   * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
    */
   template<typename ResponseT, typename TimeT = std::milli>
   FutureReturnCode

rclcpp/include/rclcpp/executors.hpp

@@ -28,7 +28,7 @@ namespace rclcpp
 {
 
 /// Create a default single-threaded executor and execute any immediately available work.
-// \param[in] node_ptr Shared pointer to the node to spin.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
 RCLCPP_PUBLIC
 void
 spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
@@ -38,7 +38,7 @@ void
 spin_some(rclcpp::node::Node::SharedPtr node_ptr);
 
 /// Create a default single-threaded executor and spin the specified node.
-// \param[in] node_ptr Shared pointer to the node to spin.
+/** \param[in] node_ptr Shared pointer to the node to spin. */
 RCLCPP_PUBLIC
 void
 spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
@@ -57,11 +57,13 @@ using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
 /**
  * \param[in] executor The executor which will spin the node.
  * \param[in] node_ptr The node to spin.
- * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this function
- * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
-   -1 is block forever, 0 is non-blocking.
-   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code.
- * \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT.
+ * \param[in] future The future to wait on. If `SUCCESS`, the future is safe to
+ *   access after this function
+ * \param[in] timeout Optional timeout parameter, which gets passed to
+ *   Executor::spin_node_once.
+ *   `-1` is block forever, `0` is non-blocking.
+ *   If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code.
+ * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
  */
 template<typename ResponseT, typename TimeT = std::milli>
 rclcpp::executor::FutureReturnCode

rclcpp/include/rclcpp/expand_topic_or_service_name.hpp

@@ -0,0 +1,63 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_
+#define RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_
+
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Expand a topic or service name and throw if it is not valid.
+/**
+ * This function can be used to "just" validate a topic or service name too,
+ * since expanding the topic name is required to fully validate a name.
+ *
+ * If the name is invalid, then InvalidTopicNameError is thrown or
+ * InvalidServiceNameError if is_service is true.
+ *
+ * This function can take any form of a topic or service name, i.e. it does not
+ * have to be a fully qualified name.
+ * The node name and namespace are used to expand it if necessary while
+ * validating it.
+ *
+ * Expansion is done with rcl_expand_topic_name.
+ * The validation is doen with rcl_validate_topic_name and
+ * rmw_validate_full_topic_name, so details about failures can be found in the
+ * documentation for those functions.
+ *
+ * \param name the topic or service name to be validated
+ * \param node_name the name of the node associated with the name
+ * \param namespace_ the namespace of the node associated with the name
+ * \param is_service if true InvalidServiceNameError is thrown instead
+ * \returns expanded (and validated) topic name
+ * \throws InvalidTopicNameError if name is invalid and is_service is false
+ * \throws InvalidServiceNameError if name is invalid and is_service is true
+ * \throws std::bad_alloc if memory cannot be allocated
+ * \throws RCLError if an unexpect error occurs
+ */
+RCLCPP_PUBLIC
+std::string
+expand_topic_or_service_name(
+  const std::string & name,
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool is_service = false);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__EXPAND_TOPIC_OR_SERVICE_NAME_HPP_

rclcpp/include/rclcpp/function_traits.hpp

@@ -82,6 +82,8 @@ struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(A
 template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
 #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 ...)>>
 #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/graph_listener.hpp

@@ -68,7 +68,8 @@ public:
   virtual ~GraphListener();
 
   /// Start the graph listener's listen thread if it hasn't been started.
-  /* This function is thread-safe.
+  /**
+   * This function is thread-safe.
    *
    * \throws GraphListenerShutdownError if the GraphListener is shutdown
    */
@@ -78,7 +79,7 @@ public:
   start_if_not_started();
 
   /// Add a node to the graph listener's list of nodes.
-  /*
+  /**
    * \throws GraphListenerShutdownError if the GraphListener is shutdown
    * \throws NodeAlreadyAddedError if the given node is already in the list
    * \throws std::invalid_argument if node is nullptr
@@ -90,7 +91,8 @@ public:
   add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
 
   /// Return true if the given node is in the graph listener's list of nodes.
-  /* Also return false if given nullptr.
+  /**
+   * Also return false if given nullptr.
    *
    * \throws std::system_error anything std::mutex::lock() throws
    */
@@ -100,7 +102,8 @@ public:
   has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
 
   /// Remove a node from the graph listener's list of nodes.
-  /*
+  /**
+   *
    * \throws NodeNotFoundError if the given node is not in the list
    * \throws std::invalid_argument if node is nullptr
    * \throws std::system_error anything std::mutex::lock() throws
@@ -111,7 +114,8 @@ public:
   remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
 
   /// Stop the listening thread.
-  /* The thread cannot be restarted, and the class is defunct after calling.
+  /**
+   * The thread cannot be restarted, and the class is defunct after calling.
    * This function is called by the ~GraphListener() and does nothing if
    * shutdown() was already called.
    * This function exists separately from the ~GraphListener() so that it can

rclcpp/include/rclcpp/intra_process_manager.hpp

@@ -23,6 +23,7 @@
 #include <exception>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <unordered_map>
 #include <set>
 
@@ -40,7 +41,8 @@ namespace intra_process_manager
 {
 
 /// This class facilitates intra process communication between nodes.
-/* This class is used in the creation of publishers and subscriptions.
+/**
+ * This class is used in the creation of publishers and subscriptions.
  * A singleton instance of this class is owned by a rclcpp::Context and a
  * rclcpp::Node can use an associated Context to get an instance of this class.
  * Nodes which do not have a common Context will not exchange intra process
@@ -54,7 +56,7 @@ namespace intra_process_manager
  * When a publisher is created, it advertises on the topic the user provided,
  * as well as a "shadowing" topic of type rcl_interfaces/IntraProcessMessage.
  * For instance, if the user specified the topic '/namespace/chatter', then the
- * corresponding intra process topic might be '/namespace/chatter__intra'.
+ * corresponding intra process topic might be '/namespace/chatter/_intra'.
  * The publisher is also allocated an id which is unique among all publishers
  * and subscriptions in this process.
  * Additionally, when registered with this class a ring buffer is created and
@@ -133,7 +135,8 @@ public:
   virtual ~IntraProcessManager();
 
   /// Register a subscription with the manager, returns subscriptions unique id.
-  /* In addition to generating a unique intra process id for the subscription,
+  /**
+   * In addition to generating a unique intra process id for the subscription,
    * this method also stores the topic name of the subscription.
    *
    * This method is normally called during the creation of a subscription,
@@ -149,7 +152,8 @@ public:
   add_subscription(subscription::SubscriptionBase::SharedPtr subscription);
 
   /// Unregister a subscription using the subscription's unique id.
-  /* This method does not allocate memory.
+  /**
+   * This method does not allocate memory.
    *
    * \param intra_process_subscription_id id of the subscription to remove.
    */
@@ -158,7 +162,8 @@ public:
   remove_subscription(uint64_t intra_process_subscription_id);
 
   /// Register a publisher with the manager, returns the publisher unique id.
-  /* In addition to generating and returning a unique id for the publisher,
+  /**
+   * In addition to generating and returning a unique id for the publisher,
    * this method creates internal ring buffer storage for "in-flight" intra
    * process messages which are stored when store_intra_process_message is
    * called with this publisher's unique id.
@@ -194,7 +199,8 @@ public:
   }
 
   /// Unregister a publisher using the publisher's unique id.
-  /* This method does not allocate memory.
+  /**
+   * This method does not allocate memory.
    *
    * \param intra_process_publisher_id id of the publisher to remove.
    */
@@ -203,7 +209,8 @@ public:
   remove_publisher(uint64_t intra_process_publisher_id);
 
   /// Store a message in the manager, and return the message sequence number.
-  /* The given message is stored in internal storage using the given publisher
+  /**
+   * The given message is stored in internal storage using the given publisher
    * id and the newly generated message sequence, which is also returned.
    * The combination of publisher id and message sequence number can later
    * be used with a subscription id to retrieve the message by calling
@@ -261,7 +268,8 @@ public:
   }
 
   /// Take an intra process message.
-  /* The intra_process_publisher_id and message_sequence_number parameters
+  /**
+   * The intra_process_publisher_id and message_sequence_number parameters
    * uniquely identify a message instance, which should be taken.
    *
    * The requesting_subscriptions_intra_process_id parameter is used to make
@@ -308,6 +316,7 @@ public:
     message = nullptr;
 
     size_t target_subs_size = 0;
+    std::lock_guard<std::mutex> lock(take_mutex_);
     mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->take_intra_process_message(
       intra_process_publisher_id,
       message_sequence_number,
@@ -339,6 +348,7 @@ private:
   get_next_unique_id();
 
   IntraProcessManagerImplBase::SharedPtr impl_;
+  std::mutex take_mutex_;
 };
 
 }  // namespace intra_process_manager

rclcpp/include/rclcpp/macros.hpp

@@ -18,7 +18,8 @@
 #ifndef RCLCPP__MACROS_HPP_
 #define RCLCPP__MACROS_HPP_
 
-/* Disables the copy constructor and operator= for the given class.
+/**
+ * Disables the copy constructor and operator= for the given class.
  *
  * Use in the private section of the class.
  */
@@ -26,38 +27,41 @@
   __VA_ARGS__(const __VA_ARGS__ &) = delete; \
   __VA_ARGS__ & operator=(const __VA_ARGS__ &) = delete;
 
-/* Defines aliases and static functions for using the Class with smart pointers.
+/**
+ * Defines aliases and static functions for using the Class with smart pointers.
  *
  * Use in the public section of the class.
- * Make sure to include <memory> in the header when using this.
+ * Make sure to include `<memory>` in the header when using this.
  */
 #define RCLCPP_SMART_PTR_DEFINITIONS(...) \
   RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_UNIQUE_PTR_DEFINITIONS(__VA_ARGS__)
 
-/* Defines aliases and static functions for using the Class with smart pointers.
+/**
+ * Defines aliases and static functions for using the Class with smart pointers.
  *
  * Same as RCLCPP_SMART_PTR_DEFINITIONS expect it excludes the static
  * Class::make_unique() method definition which does not work on classes which
  * are not CopyConstructable.
  *
  * Use in the public section of the class.
- * Make sure to include <memory> in the header when using this.
+ * Make sure to include `<memory>` in the header when using this.
  */
 #define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...) \
   RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \
   RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
   __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__)
 
-/* Defines aliases only for using the Class with smart pointers.
+/**
+ * Defines aliases only for using the Class with smart pointers.
  *
  * Same as RCLCPP_SMART_PTR_DEFINITIONS expect it excludes the static
  * method definitions which do not work on pure virtual classes and classes
  * which are not CopyConstructable.
  *
  * Use in the public section of the class.
- * Make sure to include <memory> in the header when using this.
+ * Make sure to include `<memory>` in the header when using this.
  */
 #define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \
   __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -39,7 +39,8 @@ public:
 };
 
 /// Ring buffer container of unique_ptr's of T, which can be accessed by a key.
-/* T must be a CopyConstructable and CopyAssignable.
+/**
+ * T must be a CopyConstructable and CopyAssignable.
  * This class can be used in a container by using the base class MappedRingBufferBase.
  * This class must have a positive, non-zero size.
  * This class cannot be resized nor can it reserve additional space after construction.
@@ -66,9 +67,11 @@ public:
   using ElemUniquePtr = std::unique_ptr<T, ElemDeleter>;
 
   /// Constructor.
-  /* The constructor will allocate memory while reserving space.
+  /**
+   * The constructor will allocate memory while reserving space.
    *
    * \param size size of the ring buffer; must be positive and non-zero.
+   * \param allocator optional custom allocator
    */
   explicit MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
   : elements_(size), head_(0)
@@ -86,7 +89,8 @@ public:
   virtual ~MappedRingBuffer() {}
 
   /// Return a copy of the value stored in the ring buffer at the given key.
-  /* The key is matched if an element in the ring buffer has a matching key.
+  /**
+   * The key is matched if an element in the ring buffer has a matching key.
    * This method will allocate in order to return a copy.
    *
    * The key is not guaranteed to be unique, see the class docs for more.
@@ -110,7 +114,8 @@ public:
   }
 
   /// Return ownership of the value stored in the ring buffer, leaving a copy.
-  /* The key is matched if an element in the ring bufer has a matching key.
+  /**
+   * The key is matched if an element in the ring bufer has a matching key.
    * This method will allocate in order to store a copy.
    *
    * The key is not guaranteed to be unique, see the class docs for more.
@@ -147,7 +152,8 @@ public:
   }
 
   /// Return ownership of the value stored in the ring buffer at the given key.
-  /* The key is matched if an element in the ring buffer has a matching key.
+  /**
+   * The key is matched if an element in the ring buffer has a matching key.
    *
    * The key is not guaranteed to be unique, see the class docs for more.
    *
@@ -169,7 +175,8 @@ public:
   }
 
   /// Insert a key-value pair, displacing an existing pair if necessary.
-  /* The key's uniqueness is not checked on insertion.
+  /**
+   * The key's uniqueness is not checked on insertion.
    * It is up to the user to ensure the key is unique.
    * This method should not allocate memory.
    *

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -28,7 +28,7 @@ namespace message_memory_strategy
 {
 
 /// Default allocation strategy for messages received by subscriptions.
-// A message memory strategy must be templated on the type of the subscription it belongs to.
+/** A message memory strategy must be templated on the type of the subscription it belongs to. */
 template<typename MessageT, typename Alloc = std::allocator<void>>
 class MessageMemoryStrategy
 {
@@ -56,14 +56,14 @@ public:
   }
 
   /// By default, dynamically allocate a new message.
-  // \return Shared pointer to the new message.
+  /** \return Shared pointer to the new message. */
   virtual std::shared_ptr<MessageT> borrow_message()
   {
     return std::allocate_shared<MessageT, MessageAlloc>(*message_allocator_.get());
   }
 
   /// Release ownership of the message, which will deallocate it if it has no more owners.
-  // \param[in] Shared pointer to the message we are returning.
+  /** \param[in] msg Shared pointer to the message we are returning. */
   virtual void return_message(std::shared_ptr<MessageT> & msg)
   {
     msg.reset();

rclcpp/include/rclcpp/node.hpp

@@ -67,33 +67,46 @@ public:
   /// Create a new node with the specified name.
   /**
    * \param[in] node_name Name of the node.
+   * \param[in] namespace_ Namespace of the node.
    * \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.
    */
   RCLCPP_PUBLIC
-  explicit Node(const std::string & node_name, bool use_intra_process_comms = false);
+  explicit Node(
+    const std::string & node_name,
+    const std::string & namespace_ = "",
+    bool use_intra_process_comms = false);
 
   /// Create a node based on the node name and a rclcpp::context::Context.
   /**
    * \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] 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.
    */
   RCLCPP_PUBLIC
   Node(
-    const std::string & node_name, rclcpp::context::Context::SharedPtr context,
+    const std::string & node_name,
+    const std::string & namespace_,
+    rclcpp::context::Context::SharedPtr context,
     bool use_intra_process_comms = false);
 
   RCLCPP_PUBLIC
   virtual ~Node();
 
   /// Get the name of the node.
-  // \return The name of the node.
+  /** \return The name of the node. */
   RCLCPP_PUBLIC
   const char *
   get_name() const;
 
+  /// Get the namespace of the node.
+  /** \return The namespace of the node. */
+  RCLCPP_PUBLIC
+  const char *
+  get_namespace() const;
+
   /// Create and return a callback group.
   RCLCPP_PUBLIC
   rclcpp::callback_group::CallbackGroup::SharedPtr
@@ -108,6 +121,7 @@ public:
   /**
    * \param[in] topic_name The topic for this publisher to publish on.
    * \param[in] qos_history_depth The depth of the publisher message queue.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created publisher.
    */
   template<
@@ -122,6 +136,7 @@ public:
   /**
    * \param[in] topic_name The topic for this publisher to publish on.
    * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created publisher.
    */
   template<
@@ -141,6 +156,7 @@ public:
    * \param[in] group The callback group for this subscription. NULL for no callback group.
    * \param[in] ignore_local_publications True to ignore local publications.
    * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created subscription.
    */
   /* TODO(jacquelinekay):
@@ -171,6 +187,7 @@ public:
    * \param[in] group The callback group for this subscription. NULL for no callback group.
    * \param[in] ignore_local_publications True to ignore local publications.
    * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
+   * \param[in] allocator Optional custom allocator.
    * \return Shared pointer to the created subscription.
    */
   /* TODO(jacquelinekay):
@@ -231,6 +248,12 @@ public:
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
 
+  template<typename ParameterT>
+  void
+  set_parameter_if_not_set(
+    const std::string & name,
+    const ParameterT & value);
+
   RCLCPP_PUBLIC
   std::vector<rclcpp::parameter::ParameterVariant>
   get_parameters(const std::vector<std::string> & names) const;
@@ -245,10 +268,36 @@ public:
     const std::string & name,
     rclcpp::parameter::ParameterVariant & parameter) const;
 
+  /// Assign the value of the parameter if set into the parameter argument.
+  /**
+   * If the parameter was not set, then the "parameter" argument is never assigned a value.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[out] parameter The output where the value of the parameter should be assigned.
+   * \returns true if the parameter was set, false otherwise
+   */
   template<typename ParameterT>
   bool
   get_parameter(const std::string & name, ParameterT & parameter) const;
 
+  /// Get the parameter value, or the "alternative value" if not set, and assign it to "value".
+  /**
+   * If the parameter was not set, then the "value" argument is assigned
+   * the "alternative_value".
+   * In all cases, the parameter remains not set after this function is called.
+   *
+   * \param[in] name The name of the parameter to get.
+   * \param[out] value The output where the value of the parameter should be assigned.
+   * \param[in] alternative_value Value to be stored in output if the parameter was not set.
+   * \returns true if the parameter was set, false otherwise
+   */
+  template<typename ParameterT>
+  bool
+  get_parameter_or(
+    const std::string & name,
+    ParameterT & value,
+    const ParameterT & alternative_value) const;
+
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::ParameterDescriptor>
   describe_parameters(const std::vector<std::string> & names) const;
@@ -263,7 +312,8 @@ public:
 
   /// Register the callback for parameter changes
   /**
-   * \param[in] User defined callback function, It is expected to atomically set parameters.
+   * \param[in] callback User defined callback function.
+   *   It is expected to atomically set parameters.
    * \note Repeated invocations of this function will overwrite previous callbacks
    */
   template<typename CallbackT>
@@ -271,9 +321,13 @@ public:
   register_param_change_callback(CallbackT && callback);
 
   RCLCPP_PUBLIC
-  std::map<std::string, std::string>
+  std::map<std::string, std::vector<std::string>>
   get_topic_names_and_types() const;
 
+  RCLCPP_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const;
+
   RCLCPP_PUBLIC
   size_t
   count_publishers(const std::string & topic_name) const;
@@ -292,7 +346,8 @@ public:
   get_graph_event();
 
   /// Wait for a graph event to occur by waiting on an Event to become set.
-  /* The given Event must be acquire through the get_graph_event() method.
+  /**
+   * The given Event must be acquire through the get_graph_event() method.
    *
    * \throws InvalidEventError if the given event is nullptr
    * \throws EventNotRegisteredError if the given event was not acquired with

rclcpp/include/rclcpp/node_impl.hpp

@@ -208,16 +208,46 @@ Node::register_param_change_callback(CallbackT && callback)
 }
 
 template<typename ParameterT>
-bool
-Node::get_parameter(const std::string & name, ParameterT & parameter) const
+void
+Node::set_parameter_if_not_set(
+  const std::string & name,
+  const ParameterT & value)
 {
-  rclcpp::parameter::ParameterVariant parameter_variant(name, parameter);
+  rclcpp::parameter::ParameterVariant parameter_variant;
+  if (!this->get_parameter(name, parameter_variant)) {
+    this->set_parameters({
+          rclcpp::parameter::ParameterVariant(name, value),
+        });
+  }
+}
+
+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);
-  parameter = parameter_variant.get_value<ParameterT>();
+  if (result) {
+    value = parameter_variant.get_value<ParameterT>();
+  }
 
   return result;
 }
 
+template<typename ParameterT>
+bool
+Node::get_parameter_or(
+  const std::string & name,
+  ParameterT & value,
+  const ParameterT & alternative_value) const
+{
+  bool got_parameter = get_parameter(name, value);
+  if (!got_parameter) {
+    value = alternative_value;
+  }
+  return got_parameter;
+}
+
 }  // namespace node
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/node_interfaces/node_base.hpp

@@ -36,7 +36,10 @@ public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
 
   RCLCPP_PUBLIC
-  NodeBase(const std::string & node_name, rclcpp::context::Context::SharedPtr context);
+  NodeBase(
+    const std::string & node_name,
+    const std::string & namespace_,
+    rclcpp::context::Context::SharedPtr context);
 
   RCLCPP_PUBLIC
   virtual
@@ -47,6 +50,11 @@ public:
   const char *
   get_name() const;
 
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_namespace() const;
+
   RCLCPP_PUBLIC
   virtual
   rclcpp::context::Context::SharedPtr

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -39,14 +39,21 @@ public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
 
   /// Return the name of the node.
-  // \return The name of the node.
+  /** \return The name of the node. */
   RCLCPP_PUBLIC
   virtual
   const char *
   get_name() const = 0;
 
+  /// Return the namespace of the node.
+  /** \return The namespace of the node. */
+  RCLCPP_PUBLIC
+  virtual
+  const char *
+  get_namespace() const = 0;
+
   /// Return the context of the node.
-  // \return SharedPtr to the node's context.
+  /** \return SharedPtr to the node's context. */
   RCLCPP_PUBLIC
   virtual
   rclcpp::context::Context::SharedPtr
@@ -65,7 +72,8 @@ public:
   get_rcl_node_handle() const = 0;
 
   /// Return the rcl_node_t node handle in a std::shared_ptr.
-  /* This handle remains valid after the Node is destroyed.
+  /**
+   * This handle remains valid after the Node is destroyed.
    * The actual rcl node is not finalized until it is out of scope everywhere.
    */
   RCLCPP_PUBLIC
@@ -74,7 +82,8 @@ public:
   get_shared_rcl_node_handle() = 0;
 
   /// Return the rcl_node_t node handle in a std::shared_ptr.
-  /* This handle remains valid after the Node is destroyed.
+  /**
+   * This handle remains valid after the Node is destroyed.
    * The actual rcl node is not finalized until it is out of scope everywhere.
    */
   RCLCPP_PUBLIC
@@ -113,7 +122,8 @@ public:
   get_associated_with_executor_atomic() = 0;
 
   /// Return guard condition that should be notified when the internal node state changes.
-  /* For example, this should be notified when a publisher is added or removed.
+  /**
+   * For example, this should be notified when a publisher is added or removed.
    *
    * \return the rcl_guard_condition_t if it is valid, else nullptr
    */
@@ -123,7 +133,7 @@ public:
   get_notify_guard_condition() = 0;
 
   /// Acquire and return a scoped lock that protects the notify guard condition.
-  /* This should be used when triggering the notify guard condition. */
+  /** This should be used when triggering the notify guard condition. */
   RCLCPP_PUBLIC
   virtual
   std::unique_lock<std::recursive_mutex>

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -1,4 +1,4 @@
-// Copyright 2016 Open Source Robotics Foundation, Inc.
+// Copyright 2016-2017 Open Source Robotics Foundation, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -56,8 +56,18 @@ public:
 
   RCLCPP_PUBLIC
   virtual
-  std::map<std::string, std::string>
-  get_topic_names_and_types() const;
+  std::map<std::string, std::vector<std::string>>
+  get_topic_names_and_types(bool no_demangle = false) const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const;
+
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::string>
+  get_node_names() const;
 
   RCLCPP_PUBLIC
   virtual
@@ -119,7 +129,7 @@ private:
   /// Weak references to graph events out on loan.
   std::vector<rclcpp::event::Event::WeakPtr> graph_events_;
   /// Number of graph events out on loan, used to determine if the graph should be monitored.
-  /* graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */
+  /** graph_users_count_ is atomic so that it can be accessed without acquiring the graph_mutex_ */
   std::atomic_size_t graph_users_count_;
 };
 

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -1,4 +1,4 @@
-// Copyright 2016 Open Source Robotics Foundation, Inc.
+// Copyright 2016-2017 Open Source Robotics Foundation, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -18,6 +18,7 @@
 #include <chrono>
 #include <map>
 #include <string>
+#include <vector>
 
 #include "rcl/guard_condition.h"
 
@@ -36,14 +37,34 @@ class NodeGraphInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface)
 
-  /// Return a map of existing topic names (string) to topic types (string).
-  /* A topic is considered to exist when at least one publisher or subscriber
+  /// Return a map of existing topic names to list of topic types.
+  /**
+   * A topic is considered to exist when at least one publisher or subscriber
    * exists for it, whether they be local or remote to this process.
+   *
+   * \param[in] no_demangle if true, topic names and types are not demangled
    */
   RCLCPP_PUBLIC
   virtual
-  std::map<std::string, std::string>
-  get_topic_names_and_types() const = 0;
+  std::map<std::string, std::vector<std::string>>
+  get_topic_names_and_types(bool no_demangle = false) const = 0;
+
+  /// Return a map of existing service names to list of service types.
+  /**
+   * A service is considered to exist when at least one service server or
+   * service client exists for it, whether they be local or remote to this
+   * process.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const = 0;
+
+  /// Return a vector of existing node names (string).
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::string>
+  get_node_names() const = 0;
 
   /// Return the number of publishers that are advertised on a given topic.
   RCLCPP_PUBLIC
@@ -64,7 +85,8 @@ public:
   get_graph_guard_condition() const = 0;
 
   /// Notify threads waiting on graph changes.
-  /* Affects threads waiting on the notify guard condition, see:
+  /**
+   * Affects threads waiting on the notify guard condition, see:
    * get_notify_guard_condition(), as well as the threads waiting on graph
    * changes using a graph Event, see: wait_for_graph_change().
    *
@@ -84,7 +106,8 @@ public:
   notify_shutdown() = 0;
 
   /// Return a graph event, which will be set anytime a graph change occurs.
-  /* The graph Event object is a loan which must be returned.
+  /**
+   * The graph Event object is a loan which must be returned.
    * The Event object is scoped and therefore to return the load just let it go
    * out of scope.
    */
@@ -94,7 +117,8 @@ public:
   get_graph_event() = 0;
 
   /// Wait for a graph event to occur by waiting on an Event to become set.
-  /* The given Event must be acquire through the get_graph_event() method.
+  /**
+   * The given Event must be acquire through the get_graph_event() method.
    *
    * \throws InvalidEventError if the given event is nullptr
    * \throws EventNotRegisteredError if the given event was not acquired with
@@ -108,7 +132,8 @@ public:
     std::chrono::nanoseconds timeout) = 0;
 
   /// Return the number of on loan graph events, see get_graph_event().
-  /* This is typically only used by the rclcpp::graph_listener::GraphListener.
+  /**
+   * This is typically only used by the rclcpp::graph_listener::GraphListener.
    */
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/parameter.hpp

@@ -216,7 +216,7 @@ private:
 };
 
 
-/* Return a json encoded version of the parameter intended for a dict. */
+/// Return a json encoded version of the parameter intended for a dict.
 RCLCPP_PUBLIC
 std::string
 _to_json_dict_entry(const ParameterVariant & param);
@@ -235,12 +235,12 @@ operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters);
 namespace std
 {
 
-/* Return a json encoded version of the parameter intended for a list. */
+/// Return a json encoded version of the parameter intended for a list.
 RCLCPP_PUBLIC
 std::string
 to_string(const rclcpp::parameter::ParameterVariant & param);
 
-/* Return a json encoded version of a vector of parameters, as a string*/
+/// 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);

rclcpp/include/rclcpp/parameter_client.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__PARAMETER_CLIENT_HPP_
 #define RCLCPP__PARAMETER_CLIENT_HPP_
 
+#include <memory>
 #include <string>
 #include <utility>
 #include <vector>
@@ -29,6 +30,7 @@
 #include "rcl_interfaces/srv/set_parameters.hpp"
 #include "rcl_interfaces/srv/set_parameters_atomically.hpp"
 #include "rclcpp/executors.hpp"
+#include "rclcpp/create_subscription.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/parameter.hpp"
@@ -46,6 +48,15 @@ class AsyncParametersClient
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient)
 
+  RCLCPP_PUBLIC
+  AsyncParametersClient(
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
   RCLCPP_PUBLIC
   AsyncParametersClient(
     const rclcpp::node::Node::SharedPtr node,
@@ -93,16 +104,52 @@ public:
       void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)
     > callback = nullptr);
 
-  template<typename CallbackT>
+  template<
+    typename CallbackT,
+    typename Alloc = std::allocator<void>,
+    typename SubscriptionT =
+    rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent, Alloc>>
   typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
   on_parameter_event(CallbackT && callback)
   {
-    return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
-      "parameter_events", std::forward<CallbackT>(callback), rmw_qos_profile_parameter_events);
+    using rclcpp::message_memory_strategy::MessageMemoryStrategy;
+    auto msg_mem_strat =
+      MessageMemoryStrategy<rcl_interfaces::msg::ParameterEvent, Alloc>::create_default();
+
+    return rclcpp::create_subscription<
+      rcl_interfaces::msg::ParameterEvent, CallbackT, Alloc, SubscriptionT>(
+      this->node_topics_interface_.get(),
+      "parameter_events",
+      std::forward<CallbackT>(callback),
+      rmw_qos_profile_default,
+      nullptr,  // group,
+      false,  // ignore_local_publications,
+      false,  // use_intra_process_comms_,
+      msg_mem_strat,
+      std::make_shared<Alloc>());
   }
 
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const;
+
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+  {
+    return wait_for_service_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
+protected:
+  RCLCPP_PUBLIC
+  bool
+  wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);
+
 private:
-  const rclcpp::node::Node::SharedPtr node_;
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_;
   rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;
   rclcpp::client::Client<rcl_interfaces::srv::GetParameterTypes>::SharedPtr
     get_parameter_types_client_;
@@ -199,6 +246,21 @@ public:
     return async_parameters_client_->on_parameter_event(std::forward<CallbackT>(callback));
   }
 
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const
+  {
+    return async_parameters_client_->service_is_ready();
+  }
+
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+  {
+    return async_parameters_client_->wait_for_service(timeout);
+  }
+
 private:
   rclcpp::executor::Executor::SharedPtr executor_;
   rclcpp::node::Node::SharedPtr node_;

rclcpp/include/rclcpp/publisher.hpp

@@ -75,29 +75,41 @@ public:
   virtual ~PublisherBase();
 
   /// Get the topic that this publisher publishes on.
-  // \return The topic name.
+  /** \return The topic name. */
   RCLCPP_PUBLIC
   const char *
   get_topic_name() const;
 
   /// Get the queue size for this publisher.
-  // \return The queue size.
+  /** \return The queue size. */
   RCLCPP_PUBLIC
   size_t
   get_queue_size() const;
 
   /// Get the global identifier for this publisher (used in rmw and by DDS).
-  // \return The gid.
+  /** \return The gid. */
   RCLCPP_PUBLIC
   const rmw_gid_t &
   get_gid() const;
 
   /// Get the global identifier for this publisher used by intra-process communication.
-  // \return The intra-process gid.
+  /** \return The intra-process gid. */
   RCLCPP_PUBLIC
   const rmw_gid_t &
   get_intra_process_gid() const;
 
+  /// Get the rcl publisher handle.
+  /** \return The rcl publisher handle. */
+  RCLCPP_PUBLIC
+  rcl_publisher_t *
+  get_publisher_handle();
+
+  /// Get the rcl publisher handle.
+  /** \return The rcl publisher handle. */
+  RCLCPP_PUBLIC
+  const rcl_publisher_t *
+  get_publisher_handle() const;
+
   /// Compare this publisher to a gid.
   /**
    * Note that this function calls the next function.
@@ -261,6 +273,15 @@ public:
     return this->publish(unique_msg);
   }
 
+  virtual void
+  publish(const MessageT * msg)
+  {
+    if (!msg) {
+      throw std::runtime_error("msg argument is nullptr");
+    }
+    return this->publish(*msg);
+  }
+
   std::shared_ptr<MessageAlloc> get_allocator() const
   {
     return message_allocator_;

rclcpp/include/rclcpp/publisher_factory.hpp

@@ -32,7 +32,8 @@ namespace rclcpp
 {
 
 /// Factory with functions used to create a MessageT specific PublisherT.
-/* This factory class is used to encapsulate the template generated functions
+/**
+ * This factory class is used to encapsulate the template generated functions
  * which are used during the creation of a Message type specific publisher
  * within a non-templated class.
  *

rclcpp/include/rclcpp/rclcpp.hpp

@@ -12,6 +12,113 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+/** \mainpage rclcpp: ROS Client Library for C++
+ *
+ * `rclcpp` provides the canonical C++ API for interacting with ROS.
+ * It consists of these main components:
+ *
+ * - Nodes
+ *   - rclcpp::node::Node
+ *   - rclcpp/node.hpp
+ * - Publisher
+ *   - rclcpp::node::Node::create_publisher()
+ *   - rclcpp::publisher::Publisher
+ *   - rclcpp::publisher::Publisher::publish()
+ *   - rclcpp/publisher.hpp
+ * - Subscription
+ *   - rclcpp::node::Node::create_subscription()
+ *   - rclcpp::subscription::Subscription
+ *   - rclcpp/subscription.hpp
+ * - Service Client
+ *   - rclcpp::node::Node::create_client()
+ *   - rclcpp::client::Client
+ *   - rclcpp/client.hpp
+ * - Service Server
+ *   - rclcpp::node::Node::create_service()
+ *   - rclcpp::service::Service
+ *   - rclcpp/service.hpp
+ * - Timer
+ *   - rclcpp::node::Node::create_wall_timer()
+ *   - rclcpp::timer::WallTimer
+ *   - rclcpp::timer::TimerBase
+ *   - rclcpp/timer.hpp
+ * - Parameters:
+ *   - rclcpp::node::Node::set_parameters()
+ *   - rclcpp::node::Node::get_parameters()
+ *   - rclcpp::node::Node::get_parameter()
+ *   - rclcpp::node::Node::describe_parameters()
+ *   - rclcpp::node::Node::list_parameters()
+ *   - rclcpp::node::Node::register_param_change_callback()
+ *   - rclcpp::parameter::ParameterVariant
+ *   - rclcpp::parameter_client::AsyncParametersClient
+ *   - rclcpp::parameter_client::SyncParametersClient
+ *   - rclcpp/parameter.hpp
+ *   - rclcpp/parameter_client.hpp
+ *   - rclcpp/parameter_service.hpp
+ * - Rate:
+ *   - rclcpp::rate::Rate
+ *   - rclcpp::rate::WallRate
+ *   - rclcpp/rate.hpp
+ *
+ * There are also some components which help control the execution of callbacks:
+ *
+ * - Executors (responsible for execution of callbacks through a blocking spin):
+ *   - rclcpp::spin()
+ *   - rclcpp::spin_some()
+ *   - rclcpp::spin_until_future_complete()
+ *   - rclcpp::executors::single_threaded_executor::SingleThreadedExecutor
+ *   - rclcpp::executors::single_threaded_executor::SingleThreadedExecutor::add_node()
+ *   - rclcpp::executors::single_threaded_executor::SingleThreadedExecutor::spin()
+ *   - rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor
+ *   - rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor::add_node()
+ *   - rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor::spin()
+ *   - rclcpp/executor.hpp
+ *   - rclcpp/executors.hpp
+ *   - rclcpp/executors/single_threaded_executor.hpp
+ *   - rclcpp/executors/multi_threaded_executor.hpp
+ * - CallbackGroups (mechanism for enforcing concurrency rules for callbacks):
+ *   - rclcpp::node::Node::create_callback_group()
+ *   - rclcpp::callback_group::CallbackGroup
+ *   - rclcpp/callback_group.hpp
+ *
+ * Additionally, there are some methods for introspecting the ROS graph:
+ *
+ * - Graph Events (a waitable event object that wakes up when the graph changes):
+ *   - rclcpp::node::Node::get_graph_event()
+ *   - rclcpp::node::Node::wait_for_graph_change()
+ *   - rclcpp::event::Event
+ * - List topic names and types:
+ *   - rclcpp::node::Node::get_topic_names_and_types()
+ * - Get the number of publishers or subscribers on a topic:
+ *   - rclcpp::node::Node::count_publishers()
+ *   - rclcpp::node::Node::count_subscribers()
+ *
+ * Finally, there are many internal API's and utilities:
+ *
+ * - Exceptions:
+ *   - rclcpp/exceptions.hpp
+ * - Allocator related items:
+ *   - rclcpp/allocator/allocator_common.hpp
+ *   - rclcpp/allocator/allocator_deleter.hpp
+ * - Memory management tools:
+ *   - rclcpp/memory_strategies.hpp
+ *   - rclcpp/memory_strategy.hpp
+ *   - rclcpp/message_memory_strategy.hpp
+ *   - rclcpp/strategies/allocator_memory_strategy.hpp
+ *   - rclcpp/strategies/message_pool_memory_strategy.hpp
+ * - Context object which is shared amongst multiple Nodes:
+ *   - rclcpp::context::Context
+ *   - rclcpp/context.hpp
+ *   - rclcpp/contexts/default_context.hpp
+ * - Various utilities:
+ *   - rclcpp/function_traits.hpp
+ *   - rclcpp/macros.hpp
+ *   - rclcpp/scope_exit.hpp
+ *   - rclcpp/time.hpp
+ *   - rclcpp/utilities.hpp
+ *   - rclcpp/visibility_control.hpp
+ */
+
 #ifndef RCLCPP__RCLCPP_HPP_
 #define RCLCPP__RCLCPP_HPP_
 
@@ -24,6 +131,7 @@
 #include "rclcpp/parameter_client.hpp"
 #include "rclcpp/parameter_service.hpp"
 #include "rclcpp/rate.hpp"
+#include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
 

rclcpp/include/rclcpp/service.hpp

@@ -25,8 +25,10 @@
 #include "rcl/service.h"
 
 #include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
@@ -58,9 +60,13 @@ public:
   get_service_name();
 
   RCLCPP_PUBLIC
-  const rcl_service_t *
+  rcl_service_t *
   get_service_handle();
 
+  RCLCPP_PUBLIC
+  const rcl_service_t *
+  get_service_handle() const;
+
   virtual std::shared_ptr<void> create_request() = 0;
   virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
   virtual void handle_request(
@@ -70,6 +76,14 @@ public:
 protected:
   RCLCPP_DISABLE_COPY(ServiceBase)
 
+  RCLCPP_PUBLIC
+  rcl_node_t *
+  get_rcl_node_handle();
+
+  RCLCPP_PUBLIC
+  const rcl_node_t *
+  get_rcl_node_handle() const;
+
   std::shared_ptr<rcl_node_t> node_handle_;
 
   rcl_service_t * service_handle_ = nullptr;
@@ -109,12 +123,25 @@ public:
     service_handle_ = new rcl_service_t;
     *service_handle_ = rcl_get_zero_initialized_service();
 
-    if (rcl_service_init(
-        service_handle_, node_handle.get(), service_type_support_handle, service_name.c_str(),
-        &service_options) != RCL_RET_OK)
-    {
-      throw std::runtime_error(std::string("could not create service: ") +
-              rcl_get_error_string_safe());
+    rcl_ret_t ret = rcl_service_init(
+      service_handle_,
+      node_handle.get(),
+      service_type_support_handle,
+      service_name.c_str(),
+      &service_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_SERVICE_NAME_INVALID) {
+        auto rcl_node_handle = get_rcl_node_handle();
+        // this will throw on any validation problem
+        rcl_reset_error();
+        expand_topic_or_service_name(
+          service_name,
+          rcl_node_get_name(rcl_node_handle),
+          rcl_node_get_namespace(rcl_node_handle),
+          true);
+      }
+
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
     }
   }
 
@@ -151,7 +178,9 @@ public:
         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_;
     }
   }
 
@@ -183,10 +212,7 @@ public:
     rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get());
 
     if (status != RCL_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to send response: ") + rcl_get_error_string_safe());
-      // *INDENT-ON*
+      rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");
     }
   }
 

rclcpp/include/rclcpp/subscription.hpp

@@ -29,10 +29,12 @@
 
 #include "rcl_interfaces/msg/intra_process_message.hpp"
 
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/message_memory_strategy.hpp"
 #include "rclcpp/any_subscription_callback.hpp"
 #include "rclcpp/type_support_decl.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -76,6 +78,10 @@ public:
   const char *
   get_topic_name() const;
 
+  RCLCPP_PUBLIC
+  rcl_subscription_t *
+  get_subscription_handle();
+
   RCLCPP_PUBLIC
   const rcl_subscription_t *
   get_subscription_handle() const;
@@ -85,7 +91,7 @@ public:
   get_intra_process_subscription_handle() const;
 
   /// Borrow a new message.
-  // \return Shared pointer to the fresh message.
+  /** \return Shared pointer to the fresh message. */
   virtual std::shared_ptr<void>
   create_message() = 0;
   /// Check if we need to handle the message, and execute the callback if we do.
@@ -97,9 +103,10 @@ public:
   handle_message(std::shared_ptr<void> & message, const rmw_message_info_t & message_info) = 0;
 
   /// Return the message borrowed in create_message.
-  // \param[in] Shared pointer to the returned message.
+  /** \param[in] message Shared pointer to the returned message. */
   virtual void
   return_message(std::shared_ptr<void> & message) = 0;
+
   virtual void
   handle_intra_process_message(
     rcl_interfaces::msg::IntraProcessMessage & ipm,
@@ -192,6 +199,8 @@ public:
     any_callback_.dispatch(typed_message, message_info);
   }
 
+  /// Return the loaned message.
+  /** \param message message to be returned */
   void return_message(std::shared_ptr<void> & message)
   {
     auto typed_message = std::static_pointer_cast<MessageT>(message);
@@ -235,16 +244,25 @@ public:
     MatchesAnyPublishersCallbackType matches_any_publisher_callback,
     const rcl_subscription_options_t & intra_process_options)
   {
-    if (rcl_subscription_init(
-        &intra_process_subscription_handle_, node_handle_.get(),
-        rclcpp::type_support::get_intra_process_message_msg_type_support(),
-        (std::string(get_topic_name()) + "__intra").c_str(),
-        &intra_process_options) != RCL_RET_OK)
-    {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(
-        std::string("could not create intra process subscription: ") + rcl_get_error_string_safe());
-      // *INDENT-ON*
+    std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra";
+    rcl_ret_t ret = rcl_subscription_init(
+      &intra_process_subscription_handle_,
+      node_handle_.get(),
+      rclcpp::type_support::get_intra_process_message_msg_type_support(),
+      intra_process_topic_name.c_str(),
+      &intra_process_options);
+    if (ret != RCL_RET_OK) {
+      if (ret == RCL_RET_TOPIC_NAME_INVALID) {
+        auto rcl_node_handle = node_handle_.get();
+        // this will throw on any validation problem
+        rcl_reset_error();
+        expand_topic_or_service_name(
+          intra_process_topic_name,
+          rcl_node_get_name(rcl_node_handle),
+          rcl_node_get_namespace(rcl_node_handle));
+      }
+
+      rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription");
     }
 
     intra_process_subscription_id_ = intra_process_subscription_id;

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -33,7 +33,8 @@ namespace rclcpp
 {
 
 /// Factory with functions used to create a Subscription<MessageT>.
-/* This factory class is used to encapsulate the template generated functions
+/**
+ * This factory class is used to encapsulate the template generated functions
  * which are used during the creation of a Message type specific subscription
  * within a non-templated class.
  *

rclcpp/include/rclcpp/time.hpp

@@ -0,0 +1,100 @@
+// 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__TIME_HPP_
+#define RCLCPP__TIME_HPP_
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/time.h"
+
+namespace rclcpp
+{
+
+class Time
+{
+public:
+  RCLCPP_PUBLIC
+  static
+  Time
+  now(rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  explicit Time(uint64_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
+
+  RCLCPP_PUBLIC
+  Time(const Time & rhs);
+
+  RCLCPP_PUBLIC
+  Time(const builtin_interfaces::msg::Time & time_msg);  // NOLINT
+
+  RCLCPP_PUBLIC
+  virtual ~Time();
+
+  RCLCPP_PUBLIC
+  operator builtin_interfaces::msg::Time() const;
+
+  RCLCPP_PUBLIC
+  void
+  operator=(const Time & rhs);
+
+  RCLCPP_PUBLIC
+  void
+  operator=(const builtin_interfaces::msg::Time & time_msg);
+
+  RCLCPP_PUBLIC
+  bool
+  operator==(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator<=(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>=(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  Time
+  operator+(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  Time
+  operator-(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  uint64_t
+  nanoseconds() const;
+
+private:
+  rcl_time_source_t rcl_time_source_;
+  rcl_time_point_t rcl_time_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__TIME_HPP_

rclcpp/include/rclcpp/timer.hpp

@@ -55,6 +55,10 @@ public:
   void
   cancel();
 
+  RCLCPP_PUBLIC
+  void
+  reset();
+
   RCLCPP_PUBLIC
   virtual void
   execute_callback() = 0;
@@ -64,13 +68,13 @@ public:
   get_timer_handle();
 
   /// Check how long the timer has until its next scheduled callback.
-  // \return A std::chrono::duration representing the relative time until the next callback.
+  /** \return A std::chrono::duration representing the relative time until the next callback. */
   RCLCPP_PUBLIC
   std::chrono::nanoseconds
   time_until_trigger();
 
   /// Is the clock steady (i.e. is the time between ticks constant?)
-  // \return True if the clock used by this timer is steady.
+  /** \return True if the clock used by this timer is steady. */
   virtual bool is_steady() = 0;
 
   /// Check if the timer is ready to trigger the callback.

rclcpp/include/rclcpp/utilities.hpp

@@ -57,7 +57,7 @@ void
 init(int argc, char * argv[]);
 
 /// Check rclcpp's status.
-// \return True if SIGINT hasn't fired yet, false otherwise.
+/** \return True if SIGINT hasn't fired yet, false otherwise. */
 RCLCPP_PUBLIC
 bool
 ok();
@@ -68,7 +68,7 @@ void
 shutdown();
 
 /// Register a function to be called when shutdown is called.
-/* Calling the callbacks is the last thing shutdown() does. */
+/** Calling the callbacks is the last thing shutdown() does. */
 RCLCPP_PUBLIC
 void
 on_shutdown(std::function<void(void)> callback);
@@ -81,7 +81,7 @@ on_shutdown(std::function<void(void)> callback);
  * that the same guard condition is not reused across waitsets (e.g., when
  * using multiple executors in the same process). Will throw an exception if
  * initialization of the guard condition fails.
- * \param[waitset] waitset Pointer to the rcl_wait_set_t that will be using the
+ * \param waitset Pointer to the rcl_wait_set_t that will be using the
  * resulting guard condition.
  * \return Pointer to the guard condition.
  */
@@ -95,7 +95,7 @@ get_sigint_guard_condition(rcl_wait_set_t * waitset);
  * to get a sigint guard condition, then you should call release_sigint_guard_condition()
  * when you're done, to free that condition.  Will throw an exception if
  * get_sigint_guard_condition() wasn't previously called for the given waitset.
- * \param[waitset] waitset Pointer to the rcl_wait_set_t that was using the
+ * \param waitset Pointer to the rcl_wait_set_t that was using the
  * resulting guard condition.
  */
 RCLCPP_PUBLIC

rclcpp/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp</name>
-  <version>0.0.0</version>
+  <version>0.0.3</version>
   <description>The ROS client library in C++.</description>
   <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
   <license>Apache License 2.0</license>
@@ -11,12 +11,15 @@
 
   <build_export_depend>rmw</build_export_depend>
 
+  <build_depend>builtin_interfaces</build_depend>
   <build_depend>rcl_interfaces</build_depend>
-  <build_depend>rmw_implementation_cmake</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>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>
@@ -28,6 +31,7 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>rmw</test_depend>
+  <test_depend>rmw_implementation_cmake</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rclcpp/rclcpp-extras.cmake

@@ -25,6 +25,5 @@ macro(_rclcpp_register_package_hook)
   endif()
 endmacro()
 
-include("${rclcpp_DIR}/get_rclcpp_information.cmake")
 include("${rclcpp_DIR}/rclcpp_create_node_main.cmake")
 include("${rclcpp_DIR}/rclcpp_register_node_plugins.cmake")

rclcpp/src/rclcpp/client.cpp

@@ -48,6 +48,12 @@ ClientBase::get_service_name() const
   return this->service_name_;
 }
 
+rcl_client_t *
+ClientBase::get_client_handle()
+{
+  return &client_handle_;
+}
+
 const rcl_client_t *
 ClientBase::get_client_handle() const
 {
@@ -70,6 +76,12 @@ bool
 ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
 {
   auto start = std::chrono::steady_clock::now();
+  // make an event to reuse, rather than create a new one each time
+  auto node_ptr = node_graph_.lock();
+  if (!node_ptr) {
+    throw InvalidNodeError();
+  }
+  auto event = node_ptr->get_graph_event();
   // check to see if the server is ready immediately
   if (this->service_is_ready()) {
     return true;
@@ -78,12 +90,6 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
     // check was non-blocking, return immediately
     return false;
   }
-  // make an event to reuse, rather than create a new one each time
-  auto node_ptr = node_graph_.lock();
-  if (!node_ptr) {
-    throw InvalidNodeError();
-  }
-  auto event = node_ptr->get_graph_event();
   // update the time even on the first loop to account for time spent in the first call
   // to this->server_is_ready()
   std::chrono::nanoseconds time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
@@ -115,6 +121,12 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
 }
 
 rcl_node_t *
+ClientBase::get_rcl_node_handle()
+{
+  return node_handle_.get();
+}
+
+const rcl_node_t *
 ClientBase::get_rcl_node_handle() const
 {
   return node_handle_.get();

rclcpp/src/rclcpp/exceptions.cpp

@@ -18,18 +18,40 @@
 #include <functional>
 #include <string>
 
+using namespace std::string_literals;
+
 namespace rclcpp
 {
 namespace exceptions
 {
 
+std::string
+NameValidationError::format_error(
+  const char * name_type,
+  const char * name,
+  const char * error_msg,
+  size_t invalid_index)
+{
+  std::string msg = "";
+  msg += "Invalid "s + name_type + ": " + error_msg + ":\n";
+  msg += "  '"s + name + "'\n";
+  msg += "   "s + std::string(invalid_index, ' ') + "^\n";
+  return msg;
+}
+
 void
-throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix, bool reset_error)
+throw_from_rcl_error(
+  rcl_ret_t ret,
+  const std::string & prefix,
+  const rcl_error_state_t * error_state,
+  void (* reset_error)())
 {
   if (RCL_RET_OK == ret) {
     throw std::invalid_argument("ret is RCL_RET_OK");
   }
-  const rcl_error_state_t * error_state = rcl_get_error_state();
+  if (!error_state) {
+    error_state = rcl_get_error_state();
+  }
   if (!error_state) {
     throw std::runtime_error("rcl error state is not set");
   }
@@ -39,7 +61,7 @@ throw_from_rcl_error(rcl_ret_t ret, const std::string & prefix, bool reset_error
   }
   RCLErrorBase base_exc(ret, error_state);
   if (reset_error) {
-    rcl_reset_error();
+    reset_error();
   }
   switch (ret) {
     case RCL_RET_BAD_ALLOC:

rclcpp/src/rclcpp/executor.cpp

@@ -333,7 +333,7 @@ Executor::execute_client(
     response.get());
   if (status == RCL_RET_OK) {
     client->handle_response(request_header, response);
-  } else if (status != RCL_RET_SERVICE_TAKE_FAILED) {
+  } 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());

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -26,7 +26,7 @@ void
 SingleThreadedExecutor::spin()
 {
   if (spinning.exchange(true)) {
-    throw std::runtime_error("spin_some() called while already spinning");
+    throw std::runtime_error("spin() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
   while (rclcpp::utilities::ok() && spinning.load()) {

rclcpp/src/rclcpp/expand_topic_or_service_name.cpp

@@ -0,0 +1,196 @@
+// 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 "rclcpp/expand_topic_or_service_name.hpp"
+
+#include <string>
+
+#include "rcl/expand_topic_name.h"
+#include "rcl/validate_topic_name.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/scope_exit.hpp"
+#include "rcutils/types/string_map.h"
+#include "rmw/error_handling.h"
+#include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
+#include "rmw/validate_full_topic_name.h"
+
+using rclcpp::exceptions::throw_from_rcl_error;
+
+std::string
+rclcpp::expand_topic_or_service_name(
+  const std::string & name,
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool is_service)
+{
+  char * expanded_topic = nullptr;
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  rcutils_allocator_t rcutils_allocator = rcutils_get_default_allocator();
+  rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map();
+
+  rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator);
+  if (rcutils_ret != RCUTILS_RET_OK) {
+    if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) {
+      throw_from_rcl_error(RCL_RET_BAD_ALLOC, "", rcutils_get_error_state(), rcutils_reset_error);
+    } else {
+      throw_from_rcl_error(RCL_RET_ERROR, "", rcutils_get_error_state(), rcutils_reset_error);
+    }
+  }
+  rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map);
+  if (ret != RCL_RET_OK) {
+    rcutils_error_state_t error_state;
+    if (rcutils_error_state_copy(rcl_get_error_state(), &error_state) != RCUTILS_RET_OK) {
+      throw std::bad_alloc();
+    }
+    auto error_state_scope_exit = rclcpp::make_scope_exit([&error_state]() {
+      rcutils_error_state_fini(&error_state);
+    });
+    // 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_ret,
+        rcutils_get_error_string_safe());
+      rcutils_reset_error();
+    }
+    throw_from_rcl_error(ret, "", &error_state);
+  }
+
+  ret = rcl_expand_topic_name(
+    name.c_str(),
+    node_name.c_str(),
+    namespace_.c_str(),
+    &substitutions_map,
+    allocator,
+    &expanded_topic);
+
+  std::string result;
+  if (ret == RCL_RET_OK) {
+    result = expanded_topic;
+    allocator.deallocate(expanded_topic, allocator.state);
+  }
+
+  rcutils_ret = rcutils_string_map_fini(&substitutions_map);
+  if (rcutils_ret != RCUTILS_RET_OK) {
+    throw_from_rcl_error(RCL_RET_ERROR, "", rcutils_get_error_state(), rcutils_reset_error);
+  }
+
+  // expansion failed
+  if (ret != RCL_RET_OK) {
+    // if invalid topic or unknown substitution
+    if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
+      rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
+      int validation_result;
+      size_t invalid_index;
+      rcl_ret_t ret = rcl_validate_topic_name(name.c_str(), &validation_result, &invalid_index);
+      if (ret != RCL_RET_OK) {
+        throw_from_rcl_error(ret);
+      }
+
+      if (validation_result == RCL_TOPIC_NAME_VALID) {
+        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()
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(
+            RCL_RET_INVALID_ARGUMENT, "failed to validate node name",
+            rmw_get_error_state(), rmw_reset_error);
+        }
+        throw_from_rcl_error(
+          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 invalid namespace
+    } else if (ret == RCL_RET_NODE_INVALID_NAMESPACE) {
+      rcl_reset_error();  // explicitly discard error from rcl_expand_topic_name()
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(
+            RCL_RET_INVALID_ARGUMENT, "failed to validate namespace",
+            rmw_get_error_state(), rmw_reset_error);
+        }
+        throw_from_rcl_error(
+          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);
+      // something else happened
+    } else {
+      throw_from_rcl_error(ret);
+    }
+  }
+
+  // expand succeeded, but full name validation may fail still
+  int validation_result;
+  size_t invalid_index;
+  rmw_ret_t rmw_ret =
+    rmw_validate_full_topic_name(result.c_str(), &validation_result, &invalid_index);
+  if (rmw_ret != RMW_RET_OK) {
+    if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+      throw_from_rcl_error(
+        RCL_RET_INVALID_ARGUMENT, "failed to validate full topic name",
+        rmw_get_error_state(), rmw_reset_error);
+    }
+    throw_from_rcl_error(
+      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(
+              result.c_str(),
+              rmw_full_topic_name_validation_result_string(validation_result),
+              invalid_index);
+    } else {
+      throw rclcpp::exceptions::InvalidTopicNameError(
+              result.c_str(),
+              rmw_full_topic_name_validation_result_string(validation_result),
+              invalid_index);
+    }
+  }
+
+  return result;
+}

rclcpp/src/rclcpp/node.cpp

@@ -33,18 +33,23 @@
 using rclcpp::node::Node;
 using rclcpp::exceptions::throw_from_rcl_error;
 
-Node::Node(const std::string & node_name, bool use_intra_process_comms)
+Node::Node(
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool use_intra_process_comms)
 : Node(
     node_name,
+    namespace_,
     rclcpp::contexts::default_context::get_global_default_context(),
     use_intra_process_comms)
 {}
 
 Node::Node(
   const std::string & node_name,
+  const std::string & namespace_,
   rclcpp::context::Context::SharedPtr context,
   bool use_intra_process_comms)
-: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, context)),
+: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, context)),
   node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
   node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
   node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
@@ -66,6 +71,12 @@ Node::get_name() const
   return node_base_->get_name();
 }
 
+const char *
+Node::get_namespace() const
+{
+  return node_base_->get_namespace();
+}
+
 rclcpp::callback_group::CallbackGroup::SharedPtr
 Node::create_callback_group(
   rclcpp::callback_group::CallbackGroupType group_type)
@@ -133,12 +144,18 @@ Node::list_parameters(
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
-std::map<std::string, std::string>
+std::map<std::string, std::vector<std::string>>
 Node::get_topic_names_and_types() const
 {
   return node_graph_->get_topic_names_and_types();
 }
 
+std::map<std::string, std::vector<std::string>>
+Node::get_service_names_and_types() const
+{
+  return node_graph_->get_service_names_and_types();
+}
+
 size_t
 Node::count_publishers(const std::string & topic_name) const
 {

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -20,6 +20,8 @@
 #include "rclcpp/node_interfaces/node_base.hpp"
 
 #include "rclcpp/exceptions.hpp"
+#include "rmw/validate_node_name.h"
+#include "rmw/validate_namespace.h"
 
 using rclcpp::exceptions::throw_from_rcl_error;
 
@@ -27,6 +29,7 @@ using rclcpp::node_interfaces::NodeBase;
 
 NodeBase::NodeBase(
   const std::string & node_name,
+  const std::string & namespace_,
   rclcpp::context::Context::SharedPtr context)
 : context_(context),
   node_handle_(nullptr),
@@ -84,11 +87,49 @@ NodeBase::NodeBase(
   rcl_node_options_t options = rcl_node_get_default_options();
   // TODO(wjwwood): pass the Allocator to the options
   options.domain_id = domain_id;
-  ret = rcl_node_init(rcl_node, node_name.c_str(), &options);
+  ret = rcl_node_init(rcl_node, 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;
+
+    if (ret == RCL_RET_NODE_INVALID_NAME) {
+      rcl_reset_error();  // discard rcl_node_init error
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_node_name(node_name.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate node name");
+        }
+        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 (ret == RCL_RET_NODE_INVALID_NAMESPACE) {
+      rcl_reset_error();  // discard rcl_node_init error
+      int validation_result;
+      size_t invalid_index;
+      rmw_ret_t rmw_ret =
+        rmw_validate_namespace(namespace_.c_str(), &validation_result, &invalid_index);
+      if (rmw_ret != RMW_RET_OK) {
+        if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+          throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate namespace");
+        }
+        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);
+    }
+
     throw_from_rcl_error(ret, "failed to initialize rcl node");
   }
 
@@ -127,6 +168,12 @@ NodeBase::get_name() const
   return rcl_node_get_name(node_handle_.get());
 }
 
+const char *
+NodeBase::get_namespace() const
+{
+  return rcl_node_get_namespace(node_handle_.get());
+}
+
 rclcpp::context::Context::SharedPtr
 NodeBase::get_context()
 {

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -16,6 +16,7 @@
 
 #include <map>
 #include <string>
+#include <vector>
 
 #include "rcl/graph.h"
 #include "rclcpp/exceptions.hpp"
@@ -44,44 +45,137 @@ NodeGraph::~NodeGraph()
   }
 }
 
-std::map<std::string, std::string>
-NodeGraph::get_topic_names_and_types() const
+std::map<std::string, std::vector<std::string>>
+NodeGraph::get_topic_names_and_types(bool no_demangle) const
 {
-  rcl_topic_names_and_types_t topic_names_and_types =
-    rcl_get_zero_initialized_topic_names_and_types();
+  rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types();
 
-  auto ret = rcl_get_topic_names_and_types(node_base_->get_rcl_node_handle(),
-      &topic_names_and_types);
-  if (ret != RMW_RET_OK) {
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto ret = rcl_get_topic_names_and_types(
+    node_base_->get_rcl_node_handle(),
+    &allocator,
+    no_demangle,
+    &topic_names_and_types);
+  if (ret != RCL_RET_OK) {
+    auto error_msg = std::string("failed to get topic names and types: ") +
+      rcl_get_error_string_safe();
+    rcl_reset_error();
+    if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) {
+      error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") +
+        rcl_get_error_string_safe();
+    }
+    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+  }
+
+  std::map<std::string, std::vector<std::string>> topics_and_types;
+  for (size_t i = 0; i < topic_names_and_types.names.size; ++i) {
+    std::string topic_name = topic_names_and_types.names.data[i];
+    for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) {
+      topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]);
+    }
+  }
+
+  ret = rcl_names_and_types_fini(&topic_names_and_types);
+  if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not get topic names and types: ") + rmw_get_error_string_safe());
+      std::string("could not destroy topic names and types: ") + rcl_get_error_string_safe());
     // *INDENT-ON*
   }
 
-  std::map<std::string, std::string> topics;
-  for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) {
-    topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i];
+  return topics_and_types;
+}
+
+std::map<std::string, std::vector<std::string>>
+NodeGraph::get_service_names_and_types() const
+{
+  rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types();
+
+  rcl_allocator_t allocator = rcl_get_default_allocator();
+  auto ret = rcl_get_service_names_and_types(
+    node_base_->get_rcl_node_handle(),
+    &allocator,
+    &service_names_and_types);
+  if (ret != RCL_RET_OK) {
+    auto error_msg = std::string("failed to get service names and types: ") +
+      rcl_get_error_string_safe();
+    rcl_reset_error();
+    if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) {
+      error_msg +=
+        std::string(", failed also to cleanup service names and types, leaking memory: ") +
+        rcl_get_error_string_safe();
+    }
+    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
   }
 
-  ret = rmw_destroy_topic_names_and_types(&topic_names_and_types);
-  if (ret != RMW_RET_OK) {
+  std::map<std::string, std::vector<std::string>> services_and_types;
+  for (size_t i = 0; i < service_names_and_types.names.size; ++i) {
+    std::string service_name = service_names_and_types.names.data[i];
+    for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) {
+      services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]);
+    }
+  }
+
+  ret = rcl_names_and_types_fini(&service_names_and_types);
+  if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe());
+      std::string("could not destroy service names and types: ") + rcl_get_error_string_safe());
     // *INDENT-ON*
   }
 
-  return topics;
+  return services_and_types;
+}
+
+std::vector<std::string>
+NodeGraph::get_node_names() const
+{
+  rcutils_string_array_t node_names_c =
+    rcutils_get_zero_initialized_string_array();
+
+  auto allocator = rcl_get_default_allocator();
+  auto ret = rcl_get_node_names(
+    node_base_->get_rcl_node_handle(),
+    allocator,
+    &node_names_c);
+  if (ret != RCL_RET_OK) {
+    auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string_safe();
+    rcl_reset_error();
+    if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) {
+      error_msg += std::string(", failed also to cleanup node names, leaking memory: ") +
+        rcl_get_error_string_safe();
+    }
+    // TODO(karsten1987): Append rcutils_error_message once it's in master
+    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]);
+  ret = rcutils_string_array_fini(&node_names_c);
+  if (ret != RCUTILS_RET_OK) {
+    // *INDENT-OFF*
+    // TODO(karsten1987): Append rcutils_error_message once it's in master
+    throw std::runtime_error(
+      std::string("could not destroy node names: "));
+    // *INDENT-ON*
+  }
+
+  return node_names;
 }
 
 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 fqdn = rclcpp::expand_topic_or_service_name(
+    topic_name,
+    rmw_node_handle->name,
+    rmw_node_handle->namespace_,
+    false);    // false = not a service
+
   size_t count;
   // TODO(wjwwood): use the rcl equivalent methods
-  auto ret = rmw_count_publishers(rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle()),
-      topic_name.c_str(), &count);
+  auto ret = rmw_count_publishers(rmw_node_handle, fqdn.c_str(), &count);
   if (ret != RMW_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
@@ -94,10 +188,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 fqdn = rclcpp::expand_topic_or_service_name(
+    topic_name,
+    rmw_node_handle->name,
+    rmw_node_handle->namespace_,
+    false);    // false = not a service
+
   size_t count;
   // TODO(wjwwood): use the rcl equivalent methods
-  auto ret = rmw_count_subscribers(rcl_node_get_rmw_handle(node_base_->get_rcl_node_handle()),
-      topic_name.c_str(), &count);
+  auto ret = rmw_count_subscribers(rmw_node_handle, fqdn.c_str(), &count);
   if (ret != RMW_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
@@ -163,13 +263,13 @@ NodeGraph::get_graph_event()
 {
   auto event = rclcpp::event::Event::make_shared();
   std::lock_guard<std::mutex> graph_changed_lock(graph_mutex_);
+  graph_events_.push_back(event);
+  graph_users_count_++;
   // on first call, add node to graph_listener_
   if (should_add_to_graph_listener_.exchange(false)) {
     graph_listener_->add_node(this);
     graph_listener_->start_if_not_started();
   }
-  graph_events_.push_back(event);
-  graph_users_count_++;
   return event;
 }
 
@@ -196,10 +296,13 @@ NodeGraph::wait_for_graph_change(
       throw EventNotRegisteredError();
     }
   }
+  auto pred = [&event]() {
+      return event->check() || !rclcpp::utilities::ok();
+    };
   std::unique_lock<std::mutex> graph_lock(graph_mutex_);
-  graph_cv_.wait_for(graph_lock, timeout, [&event]() {
-    return event->check() || !rclcpp::utilities::ok();
-  });
+  if (!pred()) {
+    graph_cv_.wait_for(graph_lock, timeout, pred);
+  }
 }
 
 size_t

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -22,6 +22,7 @@
 
 #include "rcl_interfaces/srv/list_parameters.hpp"
 #include "rclcpp/create_publisher.hpp"
+#include "rcutils/logging_macros.h"
 #include "rmw/qos_profiles.h"
 
 using rclcpp::node_interfaces::NodeParameters;
@@ -83,11 +84,18 @@ 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());
       }
     } 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());
     }
     tmp_map[p.get_name()] = p;
@@ -190,26 +198,29 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
   std::lock_guard<std::mutex> lock(mutex_);
   rcl_interfaces::msg::ListParametersResult result;
 
-  // TODO(esteve): define parameter separator, use "." for now
+  // TODO(mikaelarguedas) define parameter separator different from "/" to avoid ambiguity
+  // using "." for now
+  const char separator = '.';
   for (auto & kv : parameters_) {
-    if (((prefixes.size() == 0) &&
+    bool get_all = (prefixes.size() == 0) &&
       ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
-      (static_cast<uint64_t>(std::count(kv.first.begin(), kv.first.end(), '.')) < depth))) ||
-      (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) {
+      (static_cast<uint64_t>(std::count(kv.first.begin(), kv.first.end(), separator)) < depth));
+    bool prefix_matches = std::any_of(prefixes.cbegin(), prefixes.cend(),
+        [&kv, &depth, &separator](const std::string & prefix) {
       if (kv.first == prefix) {
         return true;
-      } else if (kv.first.find(prefix + ".") == 0) {
+      } else if (kv.first.find(prefix + separator) == 0) {
         size_t length = prefix.length();
         std::string substr = kv.first.substr(length);
         // Cast as unsigned integer to avoid warning
         return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) ||
-        (static_cast<uint64_t>(std::count(substr.begin(), substr.end(), '.')) < depth);
+        (static_cast<uint64_t>(std::count(substr.begin(), substr.end(), separator)) < depth);
       }
       return false;
-    })))
-    {
+    });
+    if (get_all || prefix_matches) {
       result.names.push_back(kv.first);
-      size_t last_separator = kv.first.find_last_of('.');
+      size_t last_separator = kv.first.find_last_of(separator);
       if (std::string::npos != last_separator) {
         std::string prefix = kv.first.substr(0, last_separator);
         if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) ==
@@ -226,5 +237,9 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
 void
 NodeParameters::register_param_change_callback(ParametersCallbackFunction callback)
 {
+  if (parameters_callback_) {
+    RCUTILS_LOG_WARN("param_change_callback already registered, "
+      "overwriting previous callback")
+  }
   parameters_callback_ = callback;
 }

rclcpp/src/rclcpp/parameter_client.cpp

@@ -19,31 +19,87 @@
 #include <string>
 #include <vector>
 
+#include "./parameter_service_names.hpp"
+
 using rclcpp::parameter_client::AsyncParametersClient;
 using rclcpp::parameter_client::SyncParametersClient;
 
+AsyncParametersClient::AsyncParametersClient(
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+  const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+  const std::string & remote_node_name,
+  const rmw_qos_profile_t & qos_profile)
+: node_topics_interface_(node_topics_interface)
+{
+  if (remote_node_name != "") {
+    remote_node_name_ = remote_node_name;
+  } else {
+    remote_node_name_ = node_base_interface->get_name();
+  }
+
+  rcl_client_options_t options = rcl_client_get_default_options();
+  options.qos = qos_profile;
+
+  using rclcpp::client::Client;
+  using rclcpp::client::ClientBase;
+
+  get_parameters_client_ = Client<rcl_interfaces::srv::GetParameters>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::get_parameters,
+    options);
+  auto get_parameters_base = std::dynamic_pointer_cast<ClientBase>(get_parameters_client_);
+  node_services_interface->add_client(get_parameters_base, nullptr);
+
+  get_parameter_types_client_ = Client<rcl_interfaces::srv::GetParameterTypes>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::get_parameter_types,
+    options);
+  auto get_parameter_types_base =
+    std::dynamic_pointer_cast<ClientBase>(get_parameter_types_client_);
+  node_services_interface->add_client(get_parameter_types_base, nullptr);
+
+  set_parameters_client_ = Client<rcl_interfaces::srv::SetParameters>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::set_parameters,
+    options);
+  auto set_parameters_base = std::dynamic_pointer_cast<ClientBase>(set_parameters_client_);
+  node_services_interface->add_client(set_parameters_base, nullptr);
+
+  list_parameters_client_ = Client<rcl_interfaces::srv::ListParameters>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::list_parameters,
+    options);
+  auto list_parameters_base = std::dynamic_pointer_cast<ClientBase>(list_parameters_client_);
+  node_services_interface->add_client(list_parameters_base, nullptr);
+
+  describe_parameters_client_ = Client<rcl_interfaces::srv::DescribeParameters>::make_shared(
+    node_base_interface.get(),
+    node_graph_interface,
+    remote_node_name_ + "/" + parameter_service_names::describe_parameters,
+    options);
+  auto describe_parameters_base =
+    std::dynamic_pointer_cast<ClientBase>(describe_parameters_client_);
+  node_services_interface->add_client(describe_parameters_base, nullptr);
+}
+
 AsyncParametersClient::AsyncParametersClient(
   const rclcpp::node::Node::SharedPtr node,
   const std::string & remote_node_name,
   const rmw_qos_profile_t & qos_profile)
-: node_(node)
-{
-  if (remote_node_name != "") {
-    remote_node_name_ = remote_node_name;
-  } else {
-    remote_node_name_ = node_->get_name();
-  }
-  get_parameters_client_ = node_->create_client<rcl_interfaces::srv::GetParameters>(
-    remote_node_name_ + "__get_parameters", qos_profile);
-  get_parameter_types_client_ = node_->create_client<rcl_interfaces::srv::GetParameterTypes>(
-    remote_node_name_ + "__get_parameter_types", qos_profile);
-  set_parameters_client_ = node_->create_client<rcl_interfaces::srv::SetParameters>(
-    remote_node_name_ + "__set_parameters", qos_profile);
-  list_parameters_client_ = node_->create_client<rcl_interfaces::srv::ListParameters>(
-    remote_node_name_ + "__list_parameters", qos_profile);
-  describe_parameters_client_ = node_->create_client<rcl_interfaces::srv::DescribeParameters>(
-    remote_node_name_ + "__describe_parameters", qos_profile);
-}
+: AsyncParametersClient(
+    node->get_node_base_interface(),
+    node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface(),
+    remote_node_name,
+    qos_profile)
+{}
 
 std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
 AsyncParametersClient::get_parameters(
@@ -229,6 +285,43 @@ AsyncParametersClient::list_parameters(
   return future_result;
 }
 
+bool
+AsyncParametersClient::service_is_ready() const
+{
+  return
+    get_parameters_client_->service_is_ready() &&
+    get_parameter_types_client_->service_is_ready() &&
+    set_parameters_client_->service_is_ready() &&
+    list_parameters_client_->service_is_ready() &&
+    describe_parameters_client_->service_is_ready();
+}
+
+bool
+AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
+{
+  const std::vector<std::shared_ptr<rclcpp::client::ClientBase>> clients = {
+    get_parameters_client_,
+    get_parameter_types_client_,
+    set_parameters_client_,
+    list_parameters_client_,
+    describe_parameters_client_
+  };
+  for (auto & client : clients) {
+    auto stamp = std::chrono::steady_clock::now();
+    if (!client->wait_for_service(timeout)) {
+      return false;
+    }
+    if (timeout > std::chrono::nanoseconds::zero()) {
+      timeout -= std::chrono::duration_cast<std::chrono::nanoseconds>(
+        std::chrono::steady_clock::now() - stamp);
+      if (timeout < std::chrono::nanoseconds::zero()) {
+        timeout = std::chrono::nanoseconds::zero();
+      }
+    }
+  }
+  return true;
+}
+
 SyncParametersClient::SyncParametersClient(
   rclcpp::node::Node::SharedPtr node,
   const rmw_qos_profile_t & qos_profile)

rclcpp/src/rclcpp/parameter_service.cpp

@@ -19,6 +19,8 @@
 #include <string>
 #include <vector>
 
+#include "./parameter_service_names.hpp"
+
 using rclcpp::parameter_service::ParameterService;
 
 ParameterService::ParameterService(
@@ -29,7 +31,7 @@ ParameterService::ParameterService(
   std::weak_ptr<rclcpp::node::Node> captured_node = node_;
   // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
   get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
-    std::string(node_->get_name()) + "__get_parameters",
+    std::string(node_->get_name()) + "/" + parameter_service_names::get_parameters,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
@@ -48,7 +50,7 @@ ParameterService::ParameterService(
   );
 
   get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
-    std::string(node_->get_name()) + "__get_parameter_types",
+    std::string(node_->get_name()) + "/" + parameter_service_names::get_parameter_types,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
@@ -68,7 +70,7 @@ ParameterService::ParameterService(
   );
 
   set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
-    std::string(node_->get_name()) + "__set_parameters",
+    std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
@@ -90,7 +92,7 @@ ParameterService::ParameterService(
 
   set_parameters_atomically_service_ =
     node_->create_service<rcl_interfaces::srv::SetParametersAtomically>(
-    std::string(node_->get_name()) + "__set_parameters_atomically",
+    std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters_atomically,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
@@ -114,7 +116,7 @@ ParameterService::ParameterService(
   );
 
   describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
-    std::string(node_->get_name()) + "__describe_parameters",
+    std::string(node_->get_name()) + "/" + parameter_service_names::describe_parameters,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
@@ -131,7 +133,7 @@ ParameterService::ParameterService(
   );
 
   list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
-    std::string(node_->get_name()) + "__list_parameters",
+    std::string(node_->get_name()) + "/" + parameter_service_names::list_parameters,
     [captured_node](
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,

rclcpp/src/rclcpp/parameter_service_names.hpp

@@ -0,0 +1,33 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__PARAMETER_SERVICE_NAMES_HPP_
+#define RCLCPP__PARAMETER_SERVICE_NAMES_HPP_
+
+namespace rclcpp
+{
+namespace parameter_service_names
+{
+
+static constexpr const char * get_parameters = "get_parameters";
+static constexpr const char * get_parameter_types = "get_parameter_types";
+static constexpr const char * set_parameters = "set_parameters";
+static constexpr const char * set_parameters_atomically = "set_parameters_atomically";
+static constexpr const char * describe_parameters = "describe_parameters";
+static constexpr const char * list_parameters = "list_parameters";
+
+}  // namespace parameter_service_names
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PARAMETER_SERVICE_NAMES_HPP_

rclcpp/src/rclcpp/publisher.cpp

@@ -28,8 +28,10 @@
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
 
 using rclcpp::publisher::PublisherBase;
 
@@ -41,25 +43,36 @@ PublisherBase::PublisherBase(
 : rcl_node_handle_(node_base->get_shared_rcl_node_handle()),
   intra_process_publisher_id_(0), store_intra_process_message_(nullptr)
 {
-  if (rcl_publisher_init(
-      &publisher_handle_, rcl_node_handle_.get(), &type_support,
-      topic.c_str(), &publisher_options) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("could not create publisher: ") +
-            rcl_get_error_string_safe());
+  rcl_ret_t ret = rcl_publisher_init(
+    &publisher_handle_,
+    rcl_node_handle_.get(),
+    &type_support,
+    topic.c_str(),
+    &publisher_options);
+  if (ret != RCL_RET_OK) {
+    if (ret == RCL_RET_TOPIC_NAME_INVALID) {
+      auto rcl_node_handle = rcl_node_handle_.get();
+      // this will throw on any validation problem
+      rcl_reset_error();
+      expand_topic_or_service_name(
+        topic,
+        rcl_node_get_name(rcl_node_handle),
+        rcl_node_get_namespace(rcl_node_handle));
+    }
+
+    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher");
   }
   // Life time of this object is tied to the publisher handle.
   rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_);
   if (!publisher_rmw_handle) {
-    throw std::runtime_error(
-            std::string("failed to get rmw handle: ") + rcl_get_error_string_safe());
+    auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string_safe();
+    rcl_reset_error();
+    throw std::runtime_error(msg);
   }
   if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) {
-    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-    throw std::runtime_error(
-      std::string("failed to get publisher gid: ") + rmw_get_error_string_safe());
-    // *INDENT-ON*
+    auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string_safe();
+    rmw_reset_error();
+    throw std::runtime_error(msg);
   }
 }
 
@@ -70,6 +83,7 @@ PublisherBase::~PublisherBase()
       stderr,
       "Error in destruction of intra process rcl publisher handle: %s\n",
       rcl_get_error_string_safe());
+    rcl_reset_error();
   }
 
   if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) {
@@ -77,6 +91,7 @@ PublisherBase::~PublisherBase()
       stderr,
       "Error in destruction of rcl publisher handle: %s\n",
       rcl_get_error_string_safe());
+    rcl_reset_error();
   }
 }
 
@@ -91,8 +106,9 @@ PublisherBase::get_queue_size() const
 {
   const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_);
   if (!publisher_options) {
-    throw std::runtime_error(
-            std::string("failed to get publisher options: ") + rcl_get_error_string_safe());
+    auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string_safe();
+    rcl_reset_error();
+    throw std::runtime_error(msg);
   }
   return publisher_options->qos.depth;
 }
@@ -109,6 +125,18 @@ PublisherBase::get_intra_process_gid() const
   return intra_process_rmw_gid_;
 }
 
+rcl_publisher_t *
+PublisherBase::get_publisher_handle()
+{
+  return &publisher_handle_;
+}
+
+const rcl_publisher_t *
+PublisherBase::get_publisher_handle() const
+{
+  return &publisher_handle_;
+}
+
 bool
 PublisherBase::operator==(const rmw_gid_t & gid) const
 {
@@ -121,14 +149,16 @@ PublisherBase::operator==(const rmw_gid_t * gid) const
   bool result = false;
   auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result);
   if (ret != RMW_RET_OK) {
-    throw std::runtime_error(
-            std::string("failed to compare gids: ") + rmw_get_error_string_safe());
+    auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe();
+    rmw_reset_error();
+    throw std::runtime_error(msg);
   }
   if (!result) {
     ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
     if (ret != RMW_RET_OK) {
-      throw std::runtime_error(
-              std::string("failed to compare gids: ") + rmw_get_error_string_safe());
+      auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe();
+      rmw_reset_error();
+      throw std::runtime_error(msg);
     }
   }
   return result;
@@ -140,15 +170,25 @@ 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";
-  if (rcl_publisher_init(
-      &intra_process_publisher_handle_, rcl_node_handle_.get(),
-      rclcpp::type_support::get_intra_process_message_msg_type_support(),
-      intra_process_topic_name.c_str(), &intra_process_options) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("could not create intra process publisher: ") +
-            rcl_get_error_string_safe());
+  auto intra_process_topic_name = std::string(this->get_topic_name()) + "/_intra";
+  rcl_ret_t ret = rcl_publisher_init(
+    &intra_process_publisher_handle_,
+    rcl_node_handle_.get(),
+    rclcpp::type_support::get_intra_process_message_msg_type_support(),
+    intra_process_topic_name.c_str(),
+    &intra_process_options);
+  if (ret != RCL_RET_OK) {
+    if (ret == RCL_RET_TOPIC_NAME_INVALID) {
+      auto rcl_node_handle = rcl_node_handle_.get();
+      // this will throw on any validation problem
+      rcl_reset_error();
+      expand_topic_or_service_name(
+        intra_process_topic_name,
+        rcl_node_get_name(rcl_node_handle),
+        rcl_node_get_namespace(rcl_node_handle));
+    }
+
+    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process publisher");
   }
 
   intra_process_publisher_id_ = intra_process_publisher_id;
@@ -157,16 +197,16 @@ PublisherBase::setup_intra_process(
   rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(
     &intra_process_publisher_handle_);
   if (publisher_rmw_handle == nullptr) {
-    throw std::runtime_error(std::string(
-              "Failed to get rmw publisher handle") + rcl_get_error_string_safe());
+    auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string_safe();
+    rcl_reset_error();
+    throw std::runtime_error(msg);
   }
-  auto ret = rmw_get_gid_for_publisher(
+  auto rmw_ret = rmw_get_gid_for_publisher(
     publisher_rmw_handle, &intra_process_rmw_gid_);
-  if (ret != RMW_RET_OK) {
-    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-    throw std::runtime_error(
-      std::string("failed to create intra process publisher gid: ") +
-      rmw_get_error_string_safe());
-    // *INDENT-ON*
+  if (rmw_ret != RMW_RET_OK) {
+    auto msg =
+      std::string("failed to create intra process publisher gid: ") + rmw_get_error_string_safe();
+    rmw_reset_error();
+    throw std::runtime_error(msg);
   }
 }

rclcpp/src/rclcpp/service.cpp

@@ -46,8 +46,26 @@ ServiceBase::get_service_name()
   return this->service_name_;
 }
 
-const rcl_service_t *
+rcl_service_t *
 ServiceBase::get_service_handle()
 {
   return service_handle_;
 }
+
+const rcl_service_t *
+ServiceBase::get_service_handle() const
+{
+  return service_handle_;
+}
+
+rcl_node_t *
+ServiceBase::get_rcl_node_handle()
+{
+  return node_handle_.get();
+}
+
+const rcl_node_t *
+ServiceBase::get_rcl_node_handle() const
+{
+  return node_handle_.get();
+}

rclcpp/src/rclcpp/subscription.cpp

@@ -18,6 +18,9 @@
 #include <memory>
 #include <string>
 
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
+
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
@@ -38,8 +41,17 @@ SubscriptionBase::SubscriptionBase(
     topic_name.c_str(),
     &subscription_options);
   if (ret != RCL_RET_OK) {
-    throw std::runtime_error(
-            std::string("could not create subscription: ") + rcl_get_error_string_safe());
+    if (ret == RCL_RET_TOPIC_NAME_INVALID) {
+      auto rcl_node_handle = node_handle_.get();
+      // this will throw on any validation problem
+      rcl_reset_error();
+      expand_topic_or_service_name(
+        topic_name,
+        rcl_node_get_name(rcl_node_handle),
+        rcl_node_get_namespace(rcl_node_handle));
+    }
+
+    rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription");
   }
 }
 
@@ -67,6 +79,12 @@ SubscriptionBase::get_topic_name() const
   return rcl_subscription_get_topic_name(&subscription_handle_);
 }
 
+rcl_subscription_t *
+SubscriptionBase::get_subscription_handle()
+{
+  return &subscription_handle_;
+}
+
 const rcl_subscription_t *
 SubscriptionBase::get_subscription_handle() const
 {

rclcpp/src/rclcpp/time.cpp

@@ -0,0 +1,250 @@
+// 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 "rclcpp/time.hpp"
+
+#include <utility>
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rcl/time.h"
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcutils/logging_macros.h"
+
+namespace
+{
+
+rcl_time_source_t
+init_time_source(rcl_time_source_type_t clock = RCL_SYSTEM_TIME)
+{
+  rcl_time_source_t time_source;
+  auto ret = rcl_time_source_init(clock, &time_source);
+
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not initialize time source");
+  }
+
+  return time_source;
+}
+
+rcl_time_point_t
+init_time_point(rcl_time_source_t & time_source)
+{
+  rcl_time_point_t time_point;
+  auto ret = rcl_time_point_init(&time_point, &time_source);
+
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not initialize time point");
+  }
+
+  return time_point;
+}
+
+}  // namespace
+
+namespace rclcpp
+{
+
+Time
+Time::now(rcl_time_source_type_t clock)
+{
+  // TODO(karsten1987): This impl throws explicitely on RCL_ROS_TIME
+  // we have to do this, because rcl_time_source_init returns RCL_RET_OK
+  // for RCL_ROS_TIME, however defaults to system time under the hood.
+  // ref: https://github.com/ros2/rcl/blob/master/rcl/src/rcl/time.c#L66-L77
+  // This section can be removed when rcl supports ROS_TIME correctly.
+  if (clock == RCL_ROS_TIME) {
+    throw std::runtime_error("RCL_ROS_TIME is currently not implemented.");
+  }
+
+  Time now(0, 0, clock);
+
+  auto ret = rcl_time_point_get_now(&now.rcl_time_);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not get current time stamp");
+  }
+
+  return now;
+}
+
+Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock)
+: rcl_time_source_(init_time_source(clock)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  if (seconds < 0) {
+    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 += nanoseconds;
+}
+
+Time::Time(uint64_t nanoseconds, rcl_time_source_type_t clock)
+: rcl_time_source_(init_time_source(clock)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  rcl_time_.nanoseconds = nanoseconds;
+}
+
+Time::Time(const Time & rhs)
+: rcl_time_source_(init_time_source(rhs.rcl_time_source_.type)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds;
+}
+
+Time::Time(const builtin_interfaces::msg::Time & time_msg)  // NOLINT
+: rcl_time_source_(init_time_source(RCL_SYSTEM_TIME)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  if (time_msg.sec < 0) {
+    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 += time_msg.nanosec;
+}
+
+Time::~Time()
+{
+  if (rcl_time_source_fini(&rcl_time_source_) != RCL_RET_OK) {
+    RCUTILS_LOG_FATAL("failed to reclaim rcl_time_source_t in destructor of rclcpp::Time")
+  }
+  if (rcl_time_point_fini(&rcl_time_) != RCL_RET_OK) {
+    RCUTILS_LOG_FATAL("failed to reclaim rcl_time_point_t in destructor of rclcpp::Time")
+  }
+}
+
+Time::operator builtin_interfaces::msg::Time() const
+{
+  builtin_interfaces::msg::Time msg_time;
+  msg_time.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_time_.nanoseconds));
+  msg_time.nanosec = static_cast<std::uint32_t>(rcl_time_.nanoseconds % (1000 * 1000 * 1000));
+  return msg_time;
+}
+
+void
+Time::operator=(const Time & rhs)
+{
+  rcl_time_source_ = init_time_source(rhs.rcl_time_source_.type);
+  rcl_time_ = init_time_point(rcl_time_source_);
+  rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds;
+}
+
+void
+Time::operator=(const builtin_interfaces::msg::Time & time_msg)
+{
+  if (time_msg.sec < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
+
+  this->rcl_time_source_ = init_time_source();
+  this->rcl_time_ = init_time_point(this->rcl_time_source_);
+
+  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(time_msg.sec));
+  rcl_time_.nanoseconds += time_msg.nanosec;
+}
+
+bool
+Time::operator==(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't compare times with different time sources");
+  }
+
+  return rcl_time_.nanoseconds == rhs.rcl_time_.nanoseconds;
+}
+
+bool
+Time::operator<(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't compare times with different time sources");
+  }
+
+  return rcl_time_.nanoseconds < rhs.rcl_time_.nanoseconds;
+}
+
+bool
+Time::operator<=(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't compare times with different time sources");
+  }
+
+  return rcl_time_.nanoseconds <= rhs.rcl_time_.nanoseconds;
+}
+
+bool
+Time::operator>=(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't compare times with different time sources");
+  }
+
+  return rcl_time_.nanoseconds >= rhs.rcl_time_.nanoseconds;
+}
+
+bool
+Time::operator>(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't compare times with different time sources");
+  }
+
+  return rcl_time_.nanoseconds > rhs.rcl_time_.nanoseconds;
+}
+
+Time
+Time::operator+(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't add times with different time sources");
+  }
+
+  auto ns = rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds;
+  if (ns < rcl_time_.nanoseconds) {
+    throw std::overflow_error("addition leads to uint64_t overflow");
+  }
+
+  return Time(rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds);
+}
+
+Time
+Time::operator-(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't add times with different time sources");
+  }
+
+  auto ns = rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds;
+  if (ns > rcl_time_.nanoseconds) {
+    throw std::underflow_error("subtraction leads to uint64_t underflow");
+  }
+
+  return Time(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
+}
+
+uint64_t
+Time::nanoseconds() const
+{
+  return rcl_time_.nanoseconds;
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/timer.cpp

@@ -40,6 +40,14 @@ TimerBase::cancel()
   }
 }
 
+void
+TimerBase::reset()
+{
+  if (rcl_timer_reset(&timer_handle_) != RCL_RET_OK) {
+    throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string_safe());
+  }
+}
+
 bool
 TimerBase::is_ready()
 {

rclcpp/src/rclcpp/utilities.cpp

@@ -50,76 +50,26 @@ static std::mutex g_interrupt_mutex;
 #ifdef HAS_SIGACTION
 static struct sigaction old_action;
 #else
-static void (* old_signal_handler)(int) = 0;
+typedef void (* signal_handler_t)(int);
+static signal_handler_t old_signal_handler = 0;
 #endif
 
-void
 #ifdef HAS_SIGACTION
-signal_handler(int signal_value, siginfo_t * siginfo, void * context)
+struct sigaction
+set_sigaction(int signal_value, const struct sigaction & action)
 #else
-signal_handler(int signal_value)
+signal_handler_t
+set_signal_handler(int signal_value, signal_handler_t signal_handler)
 #endif
 {
-  // TODO(wjwwood): remove? move to console logging at some point?
-  printf("signal_handler(%d)\n", signal_value);
 #ifdef HAS_SIGACTION
-  if (old_action.sa_flags & SA_SIGINFO) {
-    if (old_action.sa_sigaction != NULL) {
-      old_action.sa_sigaction(signal_value, siginfo, context);
-    }
-  } else {
-    // *INDENT-OFF*
-    if (
-      old_action.sa_handler != NULL &&  // Is set
-      old_action.sa_handler != SIG_DFL &&  // Is not default
-      old_action.sa_handler != SIG_IGN)  // Is not ignored
-    // *INDENT-ON*
-    {
-      old_action.sa_handler(signal_value);
-    }
-  }
-#else
-  if (old_signal_handler) {
-    old_signal_handler(signal_value);
-  }
-#endif
-  g_signal_status = signal_value;
-  {
-    std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
-    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());
-      }
-    }
-  }
-  g_is_interrupted.store(true);
-  g_interrupt_condition_variable.notify_all();
-}
-
-void
-rclcpp::utilities::init(int argc, char * argv[])
-{
-  g_is_interrupted.store(false);
-  if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
-    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-    throw std::runtime_error(
-      std::string("failed to initialize rmw implementation: ") + rcl_get_error_string_safe());
-    // *INDENT-ON*
-  }
-#ifdef HAS_SIGACTION
-  struct sigaction action;
-  memset(&action, 0, sizeof(action));
-  sigemptyset(&action.sa_mask);
-  action.sa_sigaction = ::signal_handler;
-  action.sa_flags = SA_SIGINFO;
-  ssize_t ret = sigaction(SIGINT, &action, &old_action);
+  struct sigaction old_action;
+  ssize_t ret = sigaction(signal_value, &action, &old_action);
   if (ret == -1)
 #else
-  ::old_signal_handler = std::signal(SIGINT, ::signal_handler);
+  signal_handler_t old_signal_handler = std::signal(signal_value, signal_handler);
   // NOLINTNEXTLINE(readability/braces)
-  if (::old_signal_handler == SIG_ERR)
+  if (old_signal_handler == SIG_ERR)
 #endif
   {
     const size_t error_length = 1024;
@@ -147,6 +97,95 @@ rclcpp::utilities::init(int argc, char * argv[])
       error_string);
     // *INDENT-ON*
   }
+
+#ifdef HAS_SIGACTION
+  return old_action;
+#else
+  return old_signal_handler;
+#endif
+}
+
+void
+trigger_interrupt_guard_condition(int signal_value)
+{
+  g_signal_status = signal_value;
+  {
+    std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
+    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());
+      }
+    }
+  }
+  g_is_interrupted.store(true);
+  g_interrupt_condition_variable.notify_all();
+}
+
+void
+#ifdef HAS_SIGACTION
+signal_handler(int signal_value, siginfo_t * siginfo, void * context)
+#else
+signal_handler(int signal_value)
+#endif
+{
+  // TODO(wjwwood): remove? move to console logging at some point?
+  printf("signal_handler(%d)\n", signal_value);
+
+#ifdef HAS_SIGACTION
+  if (old_action.sa_flags & SA_SIGINFO) {
+    if (old_action.sa_sigaction != NULL) {
+      old_action.sa_sigaction(signal_value, siginfo, context);
+    }
+  } else {
+    // *INDENT-OFF*
+    if (
+      old_action.sa_handler != NULL &&  // Is set
+      old_action.sa_handler != SIG_DFL &&  // Is not default
+      old_action.sa_handler != SIG_IGN)  // Is not ignored
+    // *INDENT-ON*
+    {
+      old_action.sa_handler(signal_value);
+    }
+  }
+#else
+  if (old_signal_handler) {
+    old_signal_handler(signal_value);
+  }
+#endif
+
+  trigger_interrupt_guard_condition(signal_value);
+}
+
+void
+rclcpp::utilities::init(int argc, char * argv[])
+{
+  g_is_interrupted.store(false);
+  if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
+    std::string msg = "failed to initialize rmw implementation: ";
+    msg += rcl_get_error_string_safe();
+    rcl_reset_error();
+    throw std::runtime_error(msg);
+  }
+#ifdef HAS_SIGACTION
+  struct sigaction action;
+  memset(&action, 0, sizeof(action));
+  sigemptyset(&action.sa_mask);
+  action.sa_sigaction = ::signal_handler;
+  action.sa_flags = SA_SIGINFO;
+  ::old_action = set_sigaction(SIGINT, action);
+  // Register an on_shutdown hook to restore the old action.
+  rclcpp::utilities::on_shutdown([]() {
+    set_sigaction(SIGINT, ::old_action);
+  });
+#else
+  ::old_signal_handler = set_signal_handler(SIGINT, ::signal_handler);
+  // Register an on_shutdown hook to restore the old signal handler.
+  rclcpp::utilities::on_shutdown([]() {
+    set_signal_handler(SIGINT, ::old_signal_handler);
+  });
+#endif
 }
 
 bool
@@ -161,19 +200,8 @@ static std::vector<std::function<void(void)>> on_shutdown_callbacks_;
 void
 rclcpp::utilities::shutdown()
 {
-  g_signal_status = SIGINT;
-  {
-    std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
-    for (auto & kv : g_sigint_guard_cond_handles) {
-      if (rcl_trigger_guard_condition(&(kv.second)) != RCL_RET_OK) {
-        fprintf(stderr,
-          "[rclcpp::error] failed to trigger sigint guard condition: %s\n",
-          rcl_get_error_string_safe());
-      }
-    }
-  }
-  g_is_interrupted.store(true);
-  g_interrupt_condition_variable.notify_all();
+  trigger_interrupt_guard_condition(SIGINT);
+
   {
     std::lock_guard<std::mutex> lock(on_shutdown_mutex_);
     for (auto & on_shutdown_callback : on_shutdown_callbacks_) {

rclcpp/test/test_client.cpp

@@ -0,0 +1,60 @@
+// 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/rclcpp.hpp"
+
+#include "rcl_interfaces/srv/list_parameters.hpp"
+
+class TestClient : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::node::Node::SharedPtr node;
+};
+
+/*
+   Testing client construction and destruction.
+ */
+TEST_F(TestClient, construction_and_destruction) {
+  using rcl_interfaces::srv::ListParameters;
+  {
+    auto client = node->create_client<ListParameters>("service");
+  }
+
+  {
+    ASSERT_THROW({
+      auto client = node->create_client<ListParameters>("invalid_service?");
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}

rclcpp/test/test_expand_topic_or_service_name.cpp

@@ -0,0 +1,74 @@
+// 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 "rclcpp/exceptions.hpp"
+#include "rclcpp/expand_topic_or_service_name.hpp"
+
+/*
+   Testing expand_topic_or_service_name.
+ */
+TEST(TestExpandTopicOrServiceName, normal) {
+  using rclcpp::expand_topic_or_service_name;
+  {
+    ASSERT_EQ("/ns/chatter", expand_topic_or_service_name("chatter", "node", "/ns"));
+  }
+}
+
+/*
+   Testing exceptions of expand_topic_or_service_name.
+ */
+TEST(TestExpandTopicOrServiceName, exceptions) {
+  using rclcpp::expand_topic_or_service_name;
+  {
+    ASSERT_THROW({
+      expand_topic_or_service_name("chatter", "invalid_node?", "/ns");
+    }, rclcpp::exceptions::InvalidNodeNameError);
+  }
+
+  {
+    ASSERT_THROW({
+      expand_topic_or_service_name("chatter", "node", "/invalid_ns?");
+    }, rclcpp::exceptions::InvalidNamespaceError);
+  }
+
+  {
+    ASSERT_THROW({
+      expand_topic_or_service_name("chatter/42invalid", "node", "/ns");
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+
+  {
+    ASSERT_THROW({
+      // this one will only fail on the "full" topic name validation check
+      expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns");
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+
+  {
+    ASSERT_THROW({
+      // is_service = true
+      expand_topic_or_service_name("chatter/42invalid", "node", "/ns", true);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+
+  {
+    ASSERT_THROW({
+      // is_service = true
+      // this one will only fail on the "full" topic name validation check
+      expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns", true);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}

rclcpp/test/test_externally_defined_services.cpp

@@ -83,8 +83,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) {
   // mock for externally defined service
   rcl_service_t service_handle = rcl_get_zero_initialized_service();
   rcl_service_options_t service_options = rcl_service_get_default_options();
-  const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
-    rclcpp, srv, Mock);
+  const rosidl_service_type_support_t * ts =
+    rosidl_typesupport_cpp::get_service_type_support_handle<rclcpp::srv::Mock>();
   rcl_ret_t ret = rcl_service_init(
     &service_handle,
     node_handle->get_node_base_interface()->get_rcl_node_handle(),
@@ -114,8 +114,8 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) {
   // mock for externally defined service
   rcl_service_t service_handle = rcl_get_zero_initialized_service();
   rcl_service_options_t service_options = rcl_service_get_default_options();
-  const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
-    rclcpp, srv, Mock);
+  const rosidl_service_type_support_t * ts =
+    rosidl_typesupport_cpp::get_service_type_support_handle<rclcpp::srv::Mock>();
   rcl_ret_t ret = rcl_service_init(
     &service_handle,
     node_handle->get_node_base_interface()->get_rcl_node_handle(),

rclcpp/test/test_node.cpp

@@ -0,0 +1,75 @@
+// 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/node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class TestNode : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+/*
+   Testing node construction and destruction.
+ */
+TEST_F(TestNode, construction_and_destruction) {
+  {
+    auto node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
+  }
+
+  {
+    ASSERT_THROW({
+      auto node = std::make_shared<rclcpp::node::Node>("invalid_node?", "/ns");
+    }, rclcpp::exceptions::InvalidNodeNameError);
+  }
+
+  {
+    ASSERT_THROW({
+      auto node = std::make_shared<rclcpp::node::Node>("my_node", "/invalid_ns?");
+    }, rclcpp::exceptions::InvalidNamespaceError);
+  }
+}
+
+TEST_F(TestNode, get_name_and_namespace) {
+  {
+    auto node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
+    EXPECT_STREQ("my_node", node->get_name());
+    EXPECT_STREQ("/ns", node->get_namespace());
+  }
+  {
+    auto node = std::make_shared<rclcpp::node::Node>("my_node", "ns");
+    EXPECT_STREQ("my_node", node->get_name());
+    EXPECT_STREQ("/ns", node->get_namespace());
+  }
+  {
+    auto node = std::make_shared<rclcpp::node::Node>("my_node", "/my/ns");
+    EXPECT_STREQ("my_node", node->get_name());
+    EXPECT_STREQ("/my/ns", node->get_namespace());
+  }
+  {
+    auto node = std::make_shared<rclcpp::node::Node>("my_node", "my/ns");
+    EXPECT_STREQ("my_node", node->get_name());
+    EXPECT_STREQ("/my/ns", node->get_namespace());
+  }
+}

rclcpp/test/test_publisher.cpp

@@ -0,0 +1,60 @@
+// 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/rclcpp.hpp"
+
+#include "rcl_interfaces/msg/intra_process_message.hpp"
+
+class TestPublisher : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::node::Node::SharedPtr node;
+};
+
+/*
+   Testing publisher construction and destruction.
+ */
+TEST_F(TestPublisher, construction_and_destruction) {
+  using rcl_interfaces::msg::IntraProcessMessage;
+  {
+    auto publisher = node->create_publisher<IntraProcessMessage>("topic");
+  }
+
+  {
+    ASSERT_THROW({
+      auto publisher = node->create_publisher<IntraProcessMessage>("invalid_topic?");
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+}

rclcpp/test/test_rate.cpp

@@ -19,7 +19,7 @@
 #include "rclcpp/rate.hpp"
 
 /*
-   Basic tests for the Rate and WallRate clases.
+   Basic tests for the Rate and WallRate classes.
  */
 TEST(TestRate, rate_basics) {
   auto period = std::chrono::milliseconds(100);

rclcpp/test/test_service.cpp

@@ -0,0 +1,63 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcl_interfaces/srv/list_parameters.hpp"
+
+class TestService : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::node::Node::SharedPtr node;
+};
+
+/*
+   Testing service construction and destruction.
+ */
+TEST_F(TestService, construction_and_destruction) {
+  using rcl_interfaces::srv::ListParameters;
+  auto callback =
+    [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) {
+    };
+  {
+    auto service = node->create_service<ListParameters>("service", callback);
+  }
+
+  {
+    ASSERT_THROW({
+      auto service = node->create_service<ListParameters>("invalid_service?", callback);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}

rclcpp/test/test_subscription.cpp

@@ -0,0 +1,63 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcl_interfaces/msg/intra_process_message.hpp"
+
+class TestSubscription : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::node::Node>("my_node", "/ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::node::Node::SharedPtr node;
+};
+
+/*
+   Testing subscription construction and destruction.
+ */
+TEST_F(TestSubscription, construction_and_destruction) {
+  using rcl_interfaces::msg::IntraProcessMessage;
+  auto callback = [](const IntraProcessMessage::SharedPtr msg) {
+      (void)msg;
+    };
+  {
+    auto sub = node->create_subscription<IntraProcessMessage>("topic", callback);
+  }
+
+  {
+    ASSERT_THROW({
+      auto sub = node->create_subscription<IntraProcessMessage>("invalid_topic?", callback);
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+}

rclcpp/test/test_time.cpp

@@ -0,0 +1,144 @@
+// 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 <algorithm>
+#include <limits>
+#include <string>
+
+#include "rcl/error_handling.h"
+#include "rcl/time.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+
+class TestTime : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+TEST(TestTime, time_sources) {
+  using builtin_interfaces::msg::Time;
+  // TODO(Karsten1987): Fix this test once ROS_TIME is implemented
+  EXPECT_ANY_THROW(rclcpp::Time::now(RCL_ROS_TIME));
+
+  Time system_now = rclcpp::Time::now(RCL_SYSTEM_TIME);
+  EXPECT_NE(0, system_now.sec);
+  EXPECT_NE(0u, system_now.nanosec);
+
+  Time steady_now = rclcpp::Time::now(RCL_STEADY_TIME);
+  EXPECT_NE(0, steady_now.sec);
+  EXPECT_NE(0u, steady_now.nanosec);
+
+  // default
+  Time default_now = rclcpp::Time::now();
+  EXPECT_NE(0, default_now.sec);
+  EXPECT_NE(0u, default_now.nanosec);
+}
+
+TEST(TestTime, conversions) {
+  rclcpp::Time now = rclcpp::Time::now();
+  builtin_interfaces::msg::Time now_msg = now;
+
+  rclcpp::Time now_again = now_msg;
+  EXPECT_EQ(now.nanoseconds(), now_again.nanoseconds());
+
+  builtin_interfaces::msg::Time msg;
+  msg.sec = 12345;
+  msg.nanosec = 67890;
+
+  rclcpp::Time time = msg;
+  EXPECT_EQ(
+    RCL_S_TO_NS(static_cast<uint64_t>(msg.sec)) + static_cast<uint64_t>(msg.nanosec),
+    time.nanoseconds());
+  EXPECT_EQ(static_cast<uint64_t>(msg.sec), RCL_NS_TO_S(time.nanoseconds()));
+
+  builtin_interfaces::msg::Time negative_time_msg;
+  negative_time_msg.sec = -1;
+  negative_time_msg.nanosec = 1;
+
+  EXPECT_ANY_THROW({
+    rclcpp::Time negative_time = negative_time_msg;
+  });
+
+  EXPECT_ANY_THROW(rclcpp::Time(-1, 1));
+
+  EXPECT_ANY_THROW({
+    rclcpp::Time assignment(1, 2);
+    assignment = negative_time_msg;
+  });
+}
+
+TEST(TestTime, operators) {
+  rclcpp::Time old(1, 0);
+  rclcpp::Time young(2, 0);
+
+  EXPECT_TRUE(old < young);
+  EXPECT_TRUE(young > old);
+  EXPECT_TRUE(old <= young);
+  EXPECT_TRUE(young >= old);
+  EXPECT_FALSE(young == old);
+
+  rclcpp::Time add = old + young;
+  EXPECT_EQ(add.nanoseconds(), old.nanoseconds() + young.nanoseconds());
+  EXPECT_EQ(add, old + young);
+
+  rclcpp::Time sub = young - old;
+  EXPECT_EQ(sub.nanoseconds(), young.nanoseconds() - old.nanoseconds());
+  EXPECT_EQ(sub, young - old);
+
+  rclcpp::Time system_time(0, 0, RCL_SYSTEM_TIME);
+  rclcpp::Time steady_time(0, 0, RCL_STEADY_TIME);
+
+  EXPECT_ANY_THROW((void)(system_time == steady_time));
+  EXPECT_ANY_THROW((void)(system_time <= steady_time));
+  EXPECT_ANY_THROW((void)(system_time >= steady_time));
+  EXPECT_ANY_THROW((void)(system_time < steady_time));
+  EXPECT_ANY_THROW((void)(system_time > steady_time));
+  EXPECT_ANY_THROW((void)(system_time + steady_time));
+  EXPECT_ANY_THROW((void)(system_time - steady_time));
+
+  rclcpp::Time now = rclcpp::Time::now(RCL_SYSTEM_TIME);
+  rclcpp::Time later = rclcpp::Time::now(RCL_STEADY_TIME);
+
+  EXPECT_ANY_THROW((void)(now == later));
+  EXPECT_ANY_THROW((void)(now <= later));
+  EXPECT_ANY_THROW((void)(now >= later));
+  EXPECT_ANY_THROW((void)(now < later));
+  EXPECT_ANY_THROW((void)(now > later));
+  EXPECT_ANY_THROW((void)(now + later));
+  EXPECT_ANY_THROW((void)(now - later));
+
+  for (auto time_source : {RCL_ROS_TIME, RCL_SYSTEM_TIME, RCL_STEADY_TIME}) {
+    rclcpp::Time time = rclcpp::Time(0, 0, time_source);
+    rclcpp::Time copy_constructor_time(time);
+    rclcpp::Time assignment_op_time = rclcpp::Time(1, 0, time_source);
+    assignment_op_time = time;
+
+    EXPECT_TRUE(time == copy_constructor_time);
+    EXPECT_TRUE(time == assignment_op_time);
+  }
+}
+
+TEST(TestTime, overflows) {
+  rclcpp::Time max(std::numeric_limits<uint64_t>::max());
+  rclcpp::Time one(1);
+
+  EXPECT_THROW(max + one, std::overflow_error);
+  EXPECT_THROW(one - max, std::underflow_error);
+}

rclcpp_lifecycle/CMakeLists.txt

@@ -2,49 +2,44 @@ cmake_minimum_required(VERSION 3.5)
 
 project(rclcpp_lifecycle)
 
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-std=c++14")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcl_lifecycle REQUIRED)
-find_package(rmw_implementation_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 
 include_directories(include)
 
-macro(targets)
-  get_rclcpp_information("${rmw_implementation}" "rclcpp${target_suffix}")
-  get_rcl_lifecycle_information("${rmw_implementation}" "rcl_lifecycle${target_suffix}")
+### 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
+)
+ament_target_dependencies(rclcpp_lifecycle
+  "lifecycle_msgs"
+  "rcl_lifecycle"
+  "rclcpp")
 
-  ### CPP High level library
-  add_library(rclcpp_lifecycle${target_suffix}
-    SHARED
-    src/lifecycle_node.cpp
-    src/node_interfaces/lifecycle_node_interface.cpp
-    src/state.cpp
-    src/transition.cpp
-  )
-  ament_target_dependencies(rclcpp_lifecycle${target_suffix}
-    "rclcpp${target_suffix}"
-    "rcl_lifecycle${target_suffix}"
-    "lifecycle_msgs")
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(rclcpp_lifecycle PRIVATE "RCLCPP_LIFECYCLE_BUILDING_DLL")
 
-  # Causes the visibility macros to use dllexport rather than dllimport,
-  # which is appropriate when building the dll but not consuming it.
-  target_compile_definitions(rclcpp_lifecycle${target_suffix} PRIVATE "RCLCPP_LIFECYCLE_BUILDING_DLL")
-
-  install(TARGETS
-    rclcpp_lifecycle${target_suffix}
-    ARCHIVE DESTINATION lib
-    LIBRARY DESTINATION lib
-    RUNTIME DESTINATION bin)
-endmacro()
-
-call_for_each_rmw_implementation(targets GENERATE_DEFAULT)
+install(TARGETS
+  rclcpp_lifecycle
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -86,6 +81,15 @@ if(BUILD_TESTING)
     )
     target_link_libraries(test_callback_exceptions ${PROJECT_NAME})
   endif()
+  ament_add_gtest(test_state_wrapper test/test_state_wrapper.cpp)
+  if(TARGET test_state_wrapper)
+    target_include_directories(test_state_wrapper PUBLIC
+      ${rcl_lifecycle_INCLUDE_DIRS}
+      ${rclcpp_INCLUDE_DIRS}
+      ${rclcpp_lifecycle_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_state_wrapper ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_export_dependencies(rclcpp)

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -68,22 +68,29 @@ public:
   /// Create a new lifecycle node with the specified name.
   /**
    * \param[in] node_name Name of the node.
+   * \param[in] node_name Namespace of the node.
    * \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.
    */
   RCLCPP_LIFECYCLE_PUBLIC
-  explicit LifecycleNode(const std::string & node_name, bool use_intra_process_comms = false);
+  explicit LifecycleNode(
+    const std::string & node_name,
+    const std::string & namespace_ = "",
+    bool use_intra_process_comms = false);
 
   /// Create a node based on the node name and a rclcpp::context::Context.
   /**
    * \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] 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.
    */
   RCLCPP_LIFECYCLE_PUBLIC
   LifecycleNode(
-    const std::string & node_name, rclcpp::context::Context::SharedPtr context,
+    const std::string & node_name,
+    const std::string & namespace_,
+    rclcpp::context::Context::SharedPtr context,
     bool use_intra_process_comms = false);
 
   RCLCPP_LIFECYCLE_PUBLIC
@@ -95,6 +102,12 @@ public:
   const char *
   get_name() const;
 
+  /// Get the namespace of the node.
+  // \return The namespace of the node.
+  RCLCPP_LIFECYCLE_PUBLIC
+  const char *
+  get_namespace() const;
+
   /// Create and return a callback group.
   RCLCPP_LIFECYCLE_PUBLIC
   rclcpp::callback_group::CallbackGroup::SharedPtr
@@ -268,8 +281,12 @@ public:
   register_param_change_callback(CallbackT && callback);
 
   RCLCPP_LIFECYCLE_PUBLIC
-  std::map<std::string, std::string>
-  get_topic_names_and_types() const;
+  std::map<std::string, std::vector<std::string>>
+  get_topic_names_and_types(bool no_demangle = false) const;
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  std::map<std::string, std::vector<std::string>>
+  get_service_names_and_types() const;
 
   RCLCPP_LIFECYCLE_PUBLIC
   size_t
@@ -356,26 +373,81 @@ public:
 
   RCLCPP_LIFECYCLE_PUBLIC
   const State &
-  trigger_transition(unsigned int transition_id);
+  trigger_transition(
+    const Transition & transition, rcl_lifecycle_transition_key_t & cb_return_code);
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  trigger_transition(uint8_t transition_id);
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  trigger_transition(
+    uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code);
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  configure();
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  configure(rcl_lifecycle_transition_key_t & cb_return_code);
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  cleanup();
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  cleanup(rcl_lifecycle_transition_key_t & cb_return_code);
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  activate();
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  activate(rcl_lifecycle_transition_key_t & cb_return_code);
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  deactivate();
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  deactivate(rcl_lifecycle_transition_key_t & cb_return_code);
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  shutdown();
+
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  shutdown(rcl_lifecycle_transition_key_t & cb_return_code);
 
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_configure(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
+  register_on_configure(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
+
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_cleanup(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
+  register_on_cleanup(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
+
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_shutdown(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
+  register_on_shutdown(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
+
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_activate(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
+  register_on_activate(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
+
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_deactivate(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
+  register_on_deactivate(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
+
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_error(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
+  register_on_error(std::function<rcl_lifecycle_transition_key_t(const State &)> fcn);
 
 protected:
   RCLCPP_LIFECYCLE_PUBLIC

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp

@@ -125,6 +125,15 @@ public:
     rclcpp::publisher::Publisher<MessageT, Alloc>::publish(msg);
   }
 
+  virtual void
+  publish(const MessageT * msg)
+  {
+    if (!msg) {
+      throw std::runtime_error("msg argument is nullptr");
+    }
+    this->publish(*msg);
+  }
+
   /// LifecyclePublisher pulish function
   /**
    * The publish function checks whether the communication

rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp

@@ -49,7 +49,7 @@ public:
    * \return true by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
-  virtual rcl_lifecycle_ret_t
+  virtual rcl_lifecycle_transition_key_t
   on_configure(const State & previous_state);
 
   /// Callback function for cleanup transition
@@ -57,7 +57,7 @@ public:
    * \return true by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
-  virtual rcl_lifecycle_ret_t
+  virtual rcl_lifecycle_transition_key_t
   on_cleanup(const State & previous_state);
 
   /// Callback function for shutdown transition
@@ -65,7 +65,7 @@ public:
    * \return true by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
-  virtual rcl_lifecycle_ret_t
+  virtual rcl_lifecycle_transition_key_t
   on_shutdown(const State & previous_state);
 
   /// Callback function for activate transition
@@ -73,7 +73,7 @@ public:
    * \return true by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
-  virtual rcl_lifecycle_ret_t
+  virtual rcl_lifecycle_transition_key_t
   on_activate(const State & previous_state);
 
   /// Callback function for deactivate transition
@@ -81,7 +81,7 @@ public:
    * \return true by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
-  virtual rcl_lifecycle_ret_t
+  virtual rcl_lifecycle_transition_key_t
   on_deactivate(const State & previous_state);
 
   /// Callback function for errorneous transition
@@ -89,7 +89,7 @@ public:
    * \return false by default
    */
   RCLCPP_LIFECYCLE_PUBLIC
-  virtual rcl_lifecycle_ret_t
+  virtual rcl_lifecycle_transition_key_t
   on_error(const State & previous_state);
 };
 

rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp

@@ -32,7 +32,7 @@ public:
   State();
 
   RCLCPP_LIFECYCLE_PUBLIC
-  State(unsigned int id, const std::string & label);
+  State(uint8_t id, const std::string & label);
 
   RCLCPP_LIFECYCLE_PUBLIC
   explicit State(const rcl_lifecycle_state_t * rcl_lifecycle_state_handle);
@@ -41,7 +41,7 @@ public:
   virtual ~State();
 
   RCLCPP_LIFECYCLE_PUBLIC
-  unsigned int
+  uint8_t
   id() const;
 
   RCLCPP_LIFECYCLE_PUBLIC

rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp

@@ -33,11 +33,11 @@ public:
   Transition() = delete;
 
   RCLCPP_LIFECYCLE_PUBLIC
-  explicit Transition(unsigned int id, const std::string & label = "");
+  explicit Transition(uint8_t id, const std::string & label = "");
 
   RCLCPP_LIFECYCLE_PUBLIC
   Transition(
-    unsigned int id, const std::string & label,
+    uint8_t id, const std::string & label,
     State && start, State && goal);
 
   RCLCPP_LIFECYCLE_PUBLIC
@@ -47,7 +47,7 @@ public:
   virtual ~Transition();
 
   RCLCPP_LIFECYCLE_PUBLIC
-  unsigned int
+  uint8_t
   id() const;
 
   RCLCPP_LIFECYCLE_PUBLIC

rclcpp_lifecycle/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>rclcpp_lifecycle</name>
-  <version>0.0.0</version>
+  <version>0.0.3</version>
   <description>Package containing a prototype for lifecycle implementation</description>
   <maintainer email="karsten@osrfoundation.org">Karsten Knese</maintainer>
   <license>Apache License 2.0</license>
@@ -13,7 +13,6 @@
   <build_depend>rclcpp</build_depend>
   <build_depend>rcl_lifecycle</build_depend>
   <build_depend>rmw_implementation</build_depend>
-  <build_depend>rmw_implementation_cmake</build_depend>
   <build_depend>rosidl_default_generators</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>lifecycle_msgs</build_depend>

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -38,18 +38,23 @@
 namespace rclcpp_lifecycle
 {
 
-LifecycleNode::LifecycleNode(const std::string & node_name, bool use_intra_process_comms)
+LifecycleNode::LifecycleNode(
+  const std::string & node_name,
+  const std::string & namespace_,
+  bool use_intra_process_comms)
 : LifecycleNode(
     node_name,
+    namespace_,
     rclcpp::contexts::default_context::get_global_default_context(),
     use_intra_process_comms)
 {}
 
 LifecycleNode::LifecycleNode(
   const std::string & node_name,
+  const std::string & namespace_,
   rclcpp::context::Context::SharedPtr context,
   bool use_intra_process_comms)
-: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, context)),
+: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, context)),
   node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
   node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
   node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
@@ -84,6 +89,12 @@ LifecycleNode::get_name() const
   return node_base_->get_name();
 }
 
+const char *
+LifecycleNode::get_namespace() const
+{
+  return node_base_->get_namespace();
+}
+
 rclcpp::callback_group::CallbackGroup::SharedPtr
 LifecycleNode::create_callback_group(
   rclcpp::callback_group::CallbackGroupType group_type)
@@ -151,10 +162,16 @@ LifecycleNode::list_parameters(
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
-std::map<std::string, std::string>
-LifecycleNode::get_topic_names_and_types() const
+std::map<std::string, std::vector<std::string>>
+LifecycleNode::get_topic_names_and_types(bool no_demangle) const
 {
-  return node_graph_->get_topic_names_and_types();
+  return node_graph_->get_topic_names_and_types(no_demangle);
+}
+
+std::map<std::string, std::vector<std::string>>
+LifecycleNode::get_service_names_and_types() const
+{
+  return node_graph_->get_service_names_and_types();
 }
 
 size_t
@@ -228,40 +245,51 @@ LifecycleNode::get_node_parameters_interface()
 
 ////
 bool
-LifecycleNode::register_on_configure(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
+LifecycleNode::register_on_configure(
+  std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
 {
-  return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn);
+  return impl_->register_callback(
+    lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn);
 }
 
 bool
-LifecycleNode::register_on_cleanup(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
+LifecycleNode::register_on_cleanup(
+  std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
 {
-  return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn);
+  return impl_->register_callback(
+    lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn);
 }
 
 bool
-LifecycleNode::register_on_shutdown(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
+LifecycleNode::register_on_shutdown(
+  std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
 {
-  return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn);
+  return impl_->register_callback(
+    lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn);
 }
 
 bool
-LifecycleNode::register_on_activate(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
+LifecycleNode::register_on_activate(
+  std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
 {
-  return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn);
+  return impl_->register_callback(
+    lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn);
 }
 
 bool
-LifecycleNode::register_on_deactivate(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
+LifecycleNode::register_on_deactivate(
+  std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
 {
-  return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn);
+  return impl_->register_callback(
+    lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn);
 }
 
 bool
-LifecycleNode::register_on_error(std::function<rcl_lifecycle_ret_t(const State &)> fcn)
+LifecycleNode::register_on_error(
+  std::function<rcl_lifecycle_transition_key_t(const State &)> fcn)
 {
-  return impl_->register_callback(lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING,
-           fcn);
+  return impl_->register_callback(
+    lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn);
 }
 
 const State &
@@ -289,11 +317,95 @@ LifecycleNode::trigger_transition(const Transition & transition)
 }
 
 const State &
-LifecycleNode::trigger_transition(unsigned int transition_id)
+LifecycleNode::trigger_transition(
+  const Transition & transition, rcl_lifecycle_transition_key_t & cb_return_code)
+{
+  return trigger_transition(transition.id(), cb_return_code);
+}
+
+const State &
+LifecycleNode::trigger_transition(uint8_t transition_id)
 {
   return impl_->trigger_transition(transition_id);
 }
 
+const State &
+LifecycleNode::trigger_transition(
+  uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code)
+{
+  return impl_->trigger_transition(transition_id, cb_return_code);
+}
+
+const State &
+LifecycleNode::configure()
+{
+  return impl_->trigger_transition(
+    lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
+}
+
+const State &
+LifecycleNode::configure(rcl_lifecycle_transition_key_t & cb_return_code)
+{
+  return impl_->trigger_transition(
+    lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, cb_return_code);
+}
+
+const State &
+LifecycleNode::cleanup()
+{
+  return impl_->trigger_transition(
+    lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP);
+}
+
+const State &
+LifecycleNode::cleanup(rcl_lifecycle_transition_key_t & cb_return_code)
+{
+  return impl_->trigger_transition(
+    lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, cb_return_code);
+}
+
+const State &
+LifecycleNode::activate()
+{
+  return impl_->trigger_transition(
+    lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
+}
+
+const State &
+LifecycleNode::activate(rcl_lifecycle_transition_key_t & cb_return_code)
+{
+  return impl_->trigger_transition(
+    lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, cb_return_code);
+}
+
+const State &
+LifecycleNode::deactivate()
+{
+  return impl_->trigger_transition(
+    lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE);
+}
+
+const State &
+LifecycleNode::deactivate(rcl_lifecycle_transition_key_t & cb_return_code)
+{
+  return impl_->trigger_transition(
+    lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, cb_return_code);
+}
+
+const State &
+LifecycleNode::shutdown()
+{
+  return impl_->trigger_transition(
+    lifecycle_msgs::msg::Transition::TRANSITION_SHUTDOWN);
+}
+
+const State &
+LifecycleNode::shutdown(rcl_lifecycle_transition_key_t & cb_return_code)
+{
+  return impl_->trigger_transition(
+    lifecycle_msgs::msg::Transition::TRANSITION_SHUTDOWN, cb_return_code);
+}
+
 void
 LifecycleNode::add_publisher_handle(
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisherInterface> pub)

rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp

@@ -35,10 +35,13 @@
 #include "rcl/error_handling.h"
 
 #include "rcl_lifecycle/rcl_lifecycle.h"
+#include "rcl_lifecycle/transition_map.h"
 
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 
+#include "rcutils/logging_macros.h"
+
 namespace rclcpp_lifecycle
 {
 
@@ -60,18 +63,21 @@ public:
 
   ~LifecycleNodeInterfaceImpl()
   {
-    if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
-      fprintf(stderr, "%s:%u, FATAL: rcl_state_machine got destroyed externally.\n",
-        __FILE__, __LINE__);
-    } else {
-      rcl_lifecycle_state_machine_fini(&state_machine_,
-        node_base_interface_->get_rcl_node_handle());
+    rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
+    const rcl_node_options_t * node_options = rcl_node_get_options(node_handle);
+    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");
     }
   }
 
   void
   init()
   {
+    rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle();
+    const rcl_node_options_t * node_options =
+      rcl_node_get_options(node_base_interface_->get_rcl_node_handle());
     state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine();
     // The call to initialize the state machine takes
     // currently five different typesupports for all publishers/services
@@ -80,17 +86,19 @@ public:
     // the message) is done fully in RCL.
     // Services are handled in C++, so that it needs a C++ typesupport structure.
     rcl_ret_t ret = rcl_lifecycle_state_machine_init(
-      &state_machine_, node_base_interface_->get_rcl_node_handle(),
-      ROSIDL_GET_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent),
+      &state_machine_,
+      node_handle,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent),
       rosidl_typesupport_cpp::get_service_type_support_handle<ChangeStateSrv>(),
       rosidl_typesupport_cpp::get_service_type_support_handle<GetStateSrv>(),
       rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableStatesSrv>(),
       rosidl_typesupport_cpp::get_service_type_support_handle<GetAvailableTransitionsSrv>(),
-      true);
+      true,
+      &node_options->allocator);
     if (ret != RCL_RET_OK) {
       throw std::runtime_error(
-              std::string(
-                "Couldn't initialize state machine for node ") + node_base_interface_->get_name());
+              std::string("Couldn't initialize state machine for node ") +
+              node_base_interface_->get_name());
     }
 
     {  // change_state
@@ -156,7 +164,7 @@ public:
   }
 
   bool
-  register_callback(std::uint8_t lifecycle_transition, std::function<rcl_lifecycle_ret_t(
+  register_callback(std::uint8_t lifecycle_transition, std::function<rcl_lifecycle_transition_key_t(
       const State &)> & cb)
   {
     cb_map_[lifecycle_transition] = cb;
@@ -173,7 +181,14 @@ public:
       throw std::runtime_error(
               "Can't get state. State machine is not initialized.");
     }
-    resp->success = change_state(req->transition.id);
+    rcl_lifecycle_transition_key_t cb_return_code;
+    auto ret = change_state(req->transition.id, cb_return_code);
+    (void) ret;
+    // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns
+    // 1. return is the actual transition
+    // 2. return is whether an error occurred or not
+    resp->success =
+      (cb_return_code == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS);
   }
 
   void
@@ -202,7 +217,7 @@ public:
       throw std::runtime_error(
               "Can't get available states. State machine is not initialized.");
     }
-    for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) {
+    for (uint8_t i = 0; i < state_machine_.transition_map.states_size; ++i) {
       lifecycle_msgs::msg::State state;
       state.id = static_cast<uint8_t>(state_machine_.transition_map.states[i].id);
       state.label = static_cast<std::string>(state_machine_.transition_map.states[i].label);
@@ -220,10 +235,9 @@ public:
     if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
       throw std::runtime_error(
               "Can't get available transitions. State machine is not initialized.");
-      return;
     }
 
-    for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
+    for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
       rcl_lifecycle_transition_t & rcl_transition = state_machine_.transition_map.transitions[i];
       lifecycle_msgs::msg::TransitionDescription trans_desc;
       trans_desc.transition.id = rcl_transition.id;
@@ -247,7 +261,7 @@ public:
   get_available_states()
   {
     std::vector<State> states;
-    for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) {
+    for (uint8_t i = 0; i < state_machine_.transition_map.states_size; ++i) {
       State state(&state_machine_.transition_map.states[i]);
       states.push_back(state);
     }
@@ -259,7 +273,7 @@ public:
   {
     std::vector<Transition> transitions;
 
-    for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
+    for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
       Transition transition(
         &state_machine_.transition_map.transitions[i]);
       transitions.push_back(transition);
@@ -267,66 +281,68 @@ public:
     return transitions;
   }
 
-  bool
-  change_state(std::uint8_t lifecycle_transition)
+  rcl_ret_t
+  change_state(std::uint8_t lifecycle_transition, rcl_lifecycle_transition_key_t & cb_return_code)
   {
     if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
-      fprintf(stderr, "%s:%d, Unable to change state for state machine for %s: %s \n",
-        __FILE__, __LINE__, node_base_interface_->get_name(), rcl_get_error_string_safe());
-      return false;
+      RCUTILS_LOG_ERROR("Unable to change state for state machine for %s: %s",
+        node_base_interface_->get_name(), rcl_get_error_string_safe())
+      return RCL_RET_ERROR;
     }
 
     // keep the initial state to pass to a transition callback
     State initial_state(state_machine_.current_state);
 
-    unsigned int transition_id = static_cast<unsigned int>(lifecycle_transition);
+    uint8_t transition_id = lifecycle_transition;
     if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, true) != RCL_RET_OK) {
-      fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s: %s\n",
-        __FILE__, __LINE__, transition_id,
-        state_machine_.current_state->label, rcl_get_error_string_safe());
-      return false;
+      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;
     }
 
-    rcl_lifecycle_ret_t cb_success = execute_callback(
+    cb_return_code = execute_callback(
       state_machine_.current_state->id, initial_state);
 
     if (rcl_lifecycle_trigger_transition(
-        &state_machine_, cb_success, true) != RCL_RET_OK)
+        &state_machine_, cb_return_code, true) != RCL_RET_OK)
     {
-      fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n",
-        transition_id, state_machine_.current_state->label);
-      return false;
+      RCUTILS_LOG_ERROR("Failed to finish transition %u. Current state is now: %s",
+        transition_id, state_machine_.current_state->label)
+      return RCL_RET_ERROR;
     }
 
     // error handling ?!
     // TODO(karsten1987): iterate over possible ret value
-    if (cb_success == RCL_LIFECYCLE_RET_ERROR) {
-      rcl_lifecycle_ret_t error_resolved = execute_callback(state_machine_.current_state->id,
-          initial_state);
-      if (error_resolved == RCL_RET_OK) {
-        fprintf(stderr, "Exception handling was successful\n");
+    if (cb_return_code == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR) {
+      RCUTILS_LOG_WARN("Error occurred while doing error handling.")
+      rcl_lifecycle_transition_key_t error_resolved = execute_callback(
+        state_machine_.current_state->id, initial_state);
+      if (error_resolved == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) {
         // We call cleanup on the error state
-        rcl_lifecycle_trigger_transition(
-          &state_machine_, error_resolved, true);
-        fprintf(stderr, "current state after error callback%s\n",
-          state_machine_.current_state->label);
+        if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != 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
-        rcl_lifecycle_trigger_transition(
-          &state_machine_, error_resolved, true);
+        if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) {
+          RCUTILS_LOG_ERROR("Failed to call cleanup on error state")
+          return RCL_RET_ERROR;
+        }
       }
     }
     // This true holds in both cases where the actual callback
     // was successful or not, since at this point we have a valid transistion
     // to either a new primary state or error state
-    return true;
+    return RCL_RET_OK;
   }
 
-  rcl_lifecycle_ret_t
+  rcl_lifecycle_transition_key_t
   execute_callback(unsigned int cb_id, const State & previous_state)
   {
     // in case no callback was attached, we forward directly
-    auto cb_success = RCL_LIFECYCLE_RET_OK;
+    rcl_lifecycle_transition_key_t cb_success =
+      lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
 
     auto it = cb_map_.find(cb_id);
     if (it != cb_map_.end()) {
@@ -341,16 +357,25 @@ public:
         // fprintf(stderr, "Original error msg: %s\n", e.what());
         // maybe directly go for error handling here
         // and pass exception along with it
-        cb_success = RCL_LIFECYCLE_RET_ERROR;
+        cb_success = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
       }
     }
     return cb_success;
   }
 
   const State &
-  trigger_transition(unsigned int transition_id)
+  trigger_transition(uint8_t transition_id)
   {
-    change_state(transition_id);
+    rcl_lifecycle_transition_key_t error;
+    change_state(transition_id, error);
+    (void) error;
+    return get_current_state();
+  }
+
+  const State &
+  trigger_transition(uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code)
+  {
+    change_state(transition_id, cb_return_code);
     return get_current_state();
   }
 
@@ -370,7 +395,7 @@ public:
   State current_state_;
   std::map<
     std::uint8_t,
-    std::function<rcl_lifecycle_ret_t(const State &)>> cb_map_;
+    std::function<rcl_lifecycle_transition_key_t(const State &)>> cb_map_;
 
   using NodeBasePtr = std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>;
   using NodeServicesPtr = std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface>;

rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp

@@ -12,46 +12,48 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "lifecycle_msgs/msg/transition.hpp"
+
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 namespace rclcpp_lifecycle
 {
 namespace node_interfaces
 {
-rcl_lifecycle_ret_t
+rcl_lifecycle_transition_key_t
 LifecycleNodeInterface::on_configure(const State &)
 {
-  return RCL_LIFECYCLE_RET_OK;
+  return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
 }
 
-rcl_lifecycle_ret_t
+rcl_lifecycle_transition_key_t
 LifecycleNodeInterface::on_cleanup(const State &)
 {
-  return RCL_LIFECYCLE_RET_OK;
+  return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
 }
 
-rcl_lifecycle_ret_t
+rcl_lifecycle_transition_key_t
 LifecycleNodeInterface::on_shutdown(const State &)
 {
-  return RCL_LIFECYCLE_RET_OK;
+  return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
 }
 
-rcl_lifecycle_ret_t
+rcl_lifecycle_transition_key_t
 LifecycleNodeInterface::on_activate(const State &)
 {
-  return RCL_LIFECYCLE_RET_OK;
+  return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
 }
 
-rcl_lifecycle_ret_t
+rcl_lifecycle_transition_key_t
 LifecycleNodeInterface::on_deactivate(const State &)
 {
-  return RCL_LIFECYCLE_RET_OK;
+  return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
 }
 
-rcl_lifecycle_ret_t
+rcl_lifecycle_transition_key_t
 LifecycleNodeInterface::on_error(const State &)
 {
-  return RCL_LIFECYCLE_RET_FAILURE;
+  return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE;
 }
 
 }  // namespace node_interfaces

rclcpp_lifecycle/src/state.cpp

@@ -15,6 +15,9 @@
 #include "rclcpp_lifecycle/state.hpp"
 
 #include <lifecycle_msgs/msg/state.hpp>
+
+#include <rcutils/allocator.h>
+#include <rcutils/strdup.h>
 #include <rcl_lifecycle/data_types.h>
 
 #include <string>
@@ -26,7 +29,7 @@ State::State()
 : State(lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, "unknown")
 {}
 
-State::State(unsigned int id, const std::string & label)
+State::State(uint8_t id, const std::string & label)
 : owns_rcl_state_handle_(true)
 {
   if (label.empty()) {
@@ -35,7 +38,8 @@ State::State(unsigned int id, const std::string & label)
 
   auto state_handle = new rcl_lifecycle_state_t;
   state_handle->id = id;
-  state_handle->label = label.c_str();
+  state_handle->label =
+    rcutils_strndup(label.c_str(), label.size(), rcutils_get_default_allocator());
 
   state_handle_ = state_handle;
 }
@@ -53,15 +57,21 @@ State::~State()
   }
 }
 
-unsigned int
+uint8_t
 State::id() const
 {
+  if (!state_handle_) {
+    throw std::runtime_error("Error in state! Internal state_handle is NULL.");
+  }
   return state_handle_->id;
 }
 
 std::string
 State::label() const
 {
+  if (!state_handle_) {
+    throw std::runtime_error("Error in state! Internal state_handle is NULL.");
+  }
   return state_handle_->label;
 }
 

rclcpp_lifecycle/src/transition.cpp

@@ -22,7 +22,7 @@
 namespace rclcpp_lifecycle
 {
 
-Transition::Transition(unsigned int id, const std::string & label)
+Transition::Transition(uint8_t id, const std::string & label)
 : owns_rcl_transition_handle_(true)
 {
   auto transition_handle = new rcl_lifecycle_transition_t;
@@ -35,7 +35,7 @@ Transition::Transition(unsigned int id, const std::string & label)
 }
 
 Transition::Transition(
-  unsigned int id, const std::string & label,
+  uint8_t id, const std::string & label,
   State && start, State && goal)
 : owns_rcl_transition_handle_(true)
 {
@@ -75,7 +75,7 @@ Transition::~Transition()
   }
 }
 
-unsigned int
+uint8_t
 Transition::id() const
 {
   return transition_handle_->id;
@@ -90,17 +90,13 @@ Transition::label() const
 State
 Transition::start_state() const
 {
-  return State(
-    transition_handle_->start->id,
-    transition_handle_->start->label);
+  return State(transition_handle_->start);
 }
 
 State
 Transition::goal_state() const
 {
-  return State(
-    transition_handle_->goal->id,
-    transition_handle_->goal->label);
+  return State(transition_handle_->goal);
 }
 
 }  // namespace rclcpp_lifecycle

rclcpp_lifecycle/test/test_callback_exceptions.cpp

@@ -46,16 +46,18 @@ public:
   size_t number_of_callbacks = 0;
 
 protected:
-  rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_configure(const rclcpp_lifecycle::State &)
   {
     ++number_of_callbacks;
     throw std::runtime_error("custom exception raised in configure callback");
   }
 
-  rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_error(const rclcpp_lifecycle::State &)
   {
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 };
 
@@ -69,6 +71,15 @@ TEST_F(TestCallbackExceptions, positive_on_error) {
   EXPECT_EQ(static_cast<size_t>(2), test_node->number_of_callbacks);
 }
 
+TEST_F(TestCallbackExceptions, positive_on_error_with_code) {
+  auto test_node = std::make_shared<PositiveCallbackExceptionNode>("testnode");
+
+  EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
+  rcl_lifecycle_transition_key_t ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
+  test_node->configure(ret);
+  EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR, ret);
+}
+
 class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode
 {
 public:
@@ -79,18 +90,21 @@ public:
   size_t number_of_callbacks = 0;
 
 protected:
-  rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_configure(const rclcpp_lifecycle::State &)
   {
     ++number_of_callbacks;
     throw std::runtime_error("custom exception raised in configure callback");
   }
 
-  rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_error(const rclcpp_lifecycle::State &)
   {
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_FAILURE;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE;
   }
 };
+
 TEST_F(TestCallbackExceptions, negative_on_error) {
   auto test_node = std::make_shared<NegativeCallbackExceptionNode>("testnode");
 
@@ -100,3 +114,12 @@ TEST_F(TestCallbackExceptions, negative_on_error) {
   // check if all callbacks were successfully overwritten
   EXPECT_EQ(static_cast<size_t>(2), test_node->number_of_callbacks);
 }
+
+TEST_F(TestCallbackExceptions, negative_on_error_with_code) {
+  auto test_node = std::make_shared<NegativeCallbackExceptionNode>("testnode");
+
+  EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
+  rcl_lifecycle_transition_key_t ret = RCL_RET_OK;
+  test_node->configure(ret);
+  EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR, ret);
+}

rclcpp_lifecycle/test/test_lifecycle_node.cpp

@@ -29,15 +29,13 @@ using lifecycle_msgs::msg::Transition;
 
 struct GoodMood
 {
-  static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_OK;
+  static constexpr rcl_lifecycle_transition_key_t cb_ret =
+    lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
 };
 struct BadMood
 {
-  static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_FAILURE;
-};
-struct VeryBadMood
-{
-  static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_ERROR;
+  static constexpr rcl_lifecycle_transition_key_t cb_ret =
+    lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE;
 };
 
 class TestDefaultStateMachine : public ::testing::Test
@@ -68,62 +66,71 @@ public:
   size_t number_of_callbacks = 0;
 
 protected:
-  rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_configure(const rclcpp_lifecycle::State &)
   {
     EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id());
     ++number_of_callbacks;
     return Mood::cb_ret;
   }
 
-  rcl_lifecycle_ret_t on_activate(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_activate(const rclcpp_lifecycle::State &)
   {
     EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id());
     ++number_of_callbacks;
     return Mood::cb_ret;
   }
 
-  rcl_lifecycle_ret_t on_deactivate(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_deactivate(const rclcpp_lifecycle::State &)
   {
     EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id());
     ++number_of_callbacks;
     return Mood::cb_ret;
   }
 
-  rcl_lifecycle_ret_t on_cleanup(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_cleanup(const rclcpp_lifecycle::State &)
   {
     EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id());
     ++number_of_callbacks;
     return Mood::cb_ret;
   }
 
-  rcl_lifecycle_ret_t on_shutdown(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_shutdown(const rclcpp_lifecycle::State &)
   {
     EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id());
     ++number_of_callbacks;
     return Mood::cb_ret;
   }
 
-  rcl_lifecycle_ret_t on_error(const rclcpp_lifecycle::State &);
+  rcl_lifecycle_transition_key_t
+  on_error(const rclcpp_lifecycle::State &);
 };
 
 template<>
-rcl_lifecycle_ret_t MoodyLifecycleNode<GoodMood>::on_error(const rclcpp_lifecycle::State &)
+rcl_lifecycle_transition_key_t
+MoodyLifecycleNode<GoodMood>::on_error(const rclcpp_lifecycle::State &)
 {
   EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id());
   ADD_FAILURE();
-  return RCL_LIFECYCLE_RET_ERROR;
+  return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
 }
 template<>
-rcl_lifecycle_ret_t MoodyLifecycleNode<BadMood>::on_error(const rclcpp_lifecycle::State &)
+rcl_lifecycle_transition_key_t
+MoodyLifecycleNode<BadMood>::on_error(const rclcpp_lifecycle::State &)
 {
   EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id());
   ++number_of_callbacks;
-  return RCL_LIFECYCLE_RET_OK;
+  return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
 }
 
 TEST_F(TestDefaultStateMachine, empty_initializer) {
   auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
-
+  EXPECT_STREQ("testnode", test_node->get_name());
+  EXPECT_STREQ("/", test_node->get_namespace());
   EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id());
 }
 
@@ -143,6 +150,30 @@ TEST_F(TestDefaultStateMachine, trigger_transition) {
       rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id());
 }
 
+TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) {
+  auto test_node = std::make_shared<EmptyLifecycleNode>("testnode");
+
+  rcl_lifecycle_transition_key_t ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
+  test_node->configure(ret);
+  EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
+  ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
+
+  test_node->activate(ret);
+  EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
+  ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
+
+  test_node->deactivate(ret);
+  EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
+  ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
+
+  test_node->cleanup(ret);
+  EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
+  ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR;
+
+  test_node->shutdown(ret);
+  EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret);
+}
+
 TEST_F(TestDefaultStateMachine, good_mood) {
   auto test_node = std::make_shared<MoodyLifecycleNode<GoodMood>>("testnode");
 

rclcpp_lifecycle/test/test_register_custom_callbacks.cpp

@@ -46,81 +46,91 @@ public:
   size_t number_of_callbacks = 0;
 
 protected:
-  rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_configure(const rclcpp_lifecycle::State &)
   {
     ADD_FAILURE();
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 
-  rcl_lifecycle_ret_t on_activate(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_activate(const rclcpp_lifecycle::State &)
   {
     ADD_FAILURE();
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 
-  rcl_lifecycle_ret_t on_deactivate(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_deactivate(const rclcpp_lifecycle::State &)
   {
     ADD_FAILURE();
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 
-  rcl_lifecycle_ret_t on_cleanup(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_cleanup(const rclcpp_lifecycle::State &)
   {
     ADD_FAILURE();
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 
-  rcl_lifecycle_ret_t on_shutdown(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_shutdown(const rclcpp_lifecycle::State &)
   {
     ADD_FAILURE();
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 
   // Custom callbacks
 
 public:
-  rcl_lifecycle_ret_t on_custom_configure(const rclcpp_lifecycle::State & previous_state)
+  rcl_lifecycle_transition_key_t
+  on_custom_configure(const rclcpp_lifecycle::State & previous_state)
   {
     EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, previous_state.id());
     EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id());
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 
-  rcl_lifecycle_ret_t on_custom_activate(const rclcpp_lifecycle::State & previous_state)
+  rcl_lifecycle_transition_key_t
+  on_custom_activate(const rclcpp_lifecycle::State & previous_state)
   {
     EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id());
     EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id());
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 
-  rcl_lifecycle_ret_t on_custom_deactivate(const rclcpp_lifecycle::State & previous_state)
+  rcl_lifecycle_transition_key_t
+  on_custom_deactivate(const rclcpp_lifecycle::State & previous_state)
   {
     EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, previous_state.id());
     EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id());
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 
-  rcl_lifecycle_ret_t on_custom_cleanup(const rclcpp_lifecycle::State & previous_state)
+  rcl_lifecycle_transition_key_t
+  on_custom_cleanup(const rclcpp_lifecycle::State & previous_state)
   {
     EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id());
     EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id());
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 
-  rcl_lifecycle_ret_t on_custom_shutdown(const rclcpp_lifecycle::State &)
+  rcl_lifecycle_transition_key_t
+  on_custom_shutdown(const rclcpp_lifecycle::State &)
   {
     EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id());
     ++number_of_callbacks;
-    return RCL_LIFECYCLE_RET_OK;
+    return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
   }
 };
 

rclcpp_lifecycle/test/test_state_machine_info.cpp

@@ -34,40 +34,40 @@ TEST_F(TestStateMachineInfo, available_states) {
   auto test_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testnode");
   std::vector<rclcpp_lifecycle::State> available_states =
     test_node->get_available_states();
-  EXPECT_EQ((unsigned int)11, available_states.size());
+  EXPECT_EQ(11u, available_states.size());
 
   // Primary States
-  EXPECT_EQ((unsigned int)0, available_states[0].id());  // unknown
-  EXPECT_EQ((unsigned int)1, available_states[1].id());  // unconfigured
-  EXPECT_EQ((unsigned int)2, available_states[2].id());  // inactive
-  EXPECT_EQ((unsigned int)3, available_states[3].id());  // active
-  EXPECT_EQ((unsigned int)4, available_states[4].id());  // finalized
+  EXPECT_EQ(0, available_states[0].id());  // unknown
+  EXPECT_EQ(1, available_states[1].id());  // unconfigured
+  EXPECT_EQ(2, available_states[2].id());  // inactive
+  EXPECT_EQ(3, available_states[3].id());  // active
+  EXPECT_EQ(4, available_states[4].id());  // finalized
 
   // Transition States
-  EXPECT_EQ((unsigned int)10, available_states[5].id());  // configuring
-  EXPECT_EQ((unsigned int)11, available_states[6].id());  // cleaningup
-  EXPECT_EQ((unsigned int)12, available_states[7].id());  // shuttingdown
-  EXPECT_EQ((unsigned int)13, available_states[8].id());  // activating
-  EXPECT_EQ((unsigned int)14, available_states[9].id());  // deactivating
-  EXPECT_EQ((unsigned int)15, available_states[10].id());  // errorprocessing
+  EXPECT_EQ(10, available_states[5].id());  // configuring
+  EXPECT_EQ(11, available_states[6].id());  // cleaningup
+  EXPECT_EQ(12, available_states[7].id());  // shuttingdown
+  EXPECT_EQ(13, available_states[8].id());  // activating
+  EXPECT_EQ(14, available_states[9].id());  // deactivating
+  EXPECT_EQ(15, available_states[10].id());  // errorprocessing
 }
 
 TEST_F(TestStateMachineInfo, available_transitions) {
   auto test_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testnode");
   std::vector<rclcpp_lifecycle::Transition> available_transitions =
     test_node->get_available_transitions();
-  EXPECT_EQ((unsigned int)25, available_transitions.size());
+  EXPECT_EQ(25u, available_transitions.size());
   for (rclcpp_lifecycle::Transition & transition : available_transitions) {
     EXPECT_FALSE(transition.label().empty());
 
-    EXPECT_TRUE(transition.start_state().id() <= (unsigned int)4 ||
-      (transition.start_state().id() >= (unsigned int)10 &&
-      (transition.start_state().id() <= (unsigned int)15)));
+    EXPECT_TRUE(transition.start_state().id() <= 4 ||
+      (transition.start_state().id() >= 10 &&
+      (transition.start_state().id() <= 15)));
     EXPECT_FALSE(transition.start_state().label().empty());
 
-    EXPECT_TRUE(transition.goal_state().id() <= (unsigned int)4 ||
-      (transition.goal_state().id() >= (unsigned int)10 &&
-      (transition.goal_state().id() <= (unsigned int)15)));
+    EXPECT_TRUE(transition.goal_state().id() <= 4 ||
+      (transition.goal_state().id() >= 10 &&
+      (transition.goal_state().id() <= 15)));
     EXPECT_FALSE(transition.goal_state().label().empty());
   }
 }

rclcpp_lifecycle/test/test_state_wrapper.cpp

@@ -0,0 +1,77 @@
+// 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 <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+class TestStateWrapper : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+  }
+};
+
+TEST_F(TestStateWrapper, wrapper) {
+  {
+    rclcpp_lifecycle::State state(1, "my_state");
+    EXPECT_EQ(1, state.id());
+    EXPECT_FALSE(state.label().empty());
+    EXPECT_STREQ("my_state", state.label().c_str());
+  }
+
+  {
+    rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, NULL, 0};
+    rclcpp_lifecycle::State c_state(lc_state.id, lc_state.label);
+    EXPECT_EQ(2, c_state.id());
+    EXPECT_FALSE(c_state.label().empty());
+    EXPECT_STREQ("my_c_state", c_state.label().c_str());
+  }
+
+  {
+    rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, NULL, 0};
+    rclcpp_lifecycle::State c_state(&lc_state);
+    EXPECT_EQ(2, c_state.id());
+    EXPECT_FALSE(c_state.label().empty());
+    EXPECT_STREQ("my_c_state", c_state.label().c_str());
+  }
+
+  {
+    rcl_lifecycle_state_t * lc_state =
+      new rcl_lifecycle_state_t {"my_c_state", 3, NULL, NULL, 0};
+    rclcpp_lifecycle::State c_state(lc_state->id, lc_state->label);
+    EXPECT_EQ(3, c_state.id());
+    EXPECT_FALSE(c_state.label().empty());
+    EXPECT_STREQ("my_c_state", c_state.label().c_str());
+  }
+
+
+  // introduces flakiness
+  // unsupported behavior!
+  // fails when compiled with memory sanitizer
+  // {
+  //  rcl_lifecycle_state_t * lc_state
+  //    = new rcl_lifecycle_state_t {"my_c_state", 3, NULL, NULL, 0};
+  //  rclcpp_lifecycle::State c_state(lc_state);
+  //  delete lc_state;
+  //  lc_state = NULL;
+  //  EXPECT_EQ(3, c_state.id());
+  //  EXPECT_STREQ("my_c_state", c_state.label().c_str());
+  // }
+}