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/CMakeLists.txt

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

rclcpp/include/rclcpp/client.hpp

@@ -68,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;
@@ -100,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_;

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

@@ -23,6 +23,7 @@
 #include <exception>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <unordered_map>
 #include <set>
 
@@ -315,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,
@@ -346,6 +348,7 @@ private:
   get_next_unique_id();
 
   IntraProcessManagerImplBase::SharedPtr impl_;
+  std::mutex take_mutex_;
 };
 
 }  // namespace intra_process_manager

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

@@ -98,6 +98,18 @@ 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.

rclcpp/include/rclcpp/service.hpp

@@ -60,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(
@@ -74,6 +78,10 @@ protected:
 
   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_;
@@ -172,6 +180,7 @@ public:
         (std::cerr << ss.str()).flush();
         rcl_reset_error();
       }
+      delete service_handle_;
     }
   }
 

rclcpp/include/rclcpp/subscription.hpp

@@ -78,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;

rclcpp/include/rclcpp/time.hpp

@@ -15,13 +15,11 @@
 #ifndef RCLCPP__TIME_HPP_
 #define RCLCPP__TIME_HPP_
 
-#include <utility>
-
 #include "builtin_interfaces/msg/time.hpp"
-#include "rcl/time.h"
-#include "rcutils/time.h"
 
-#include "rclcpp/exceptions.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/time.h"
 
 namespace rclcpp
 {
@@ -29,42 +27,72 @@ namespace rclcpp
 class Time
 {
 public:
-  template<rcl_time_source_type_t ClockT = RCL_SYSTEM_TIME>
-  static Time
-  now()
-  {
-    rcutils_time_point_value_t rcutils_now = 0;
-    rcutils_ret_t ret = RCUTILS_RET_ERROR;
-    if (ClockT == RCL_ROS_TIME) {
-      throw std::runtime_error("RCL_ROS_TIME is currently not implemented.");
-      ret = false;
-    } else if (ClockT == RCL_SYSTEM_TIME) {
-      ret = rcutils_system_time_now(&rcutils_now);
-    } else if (ClockT == RCL_STEADY_TIME) {
-      ret = rcutils_steady_time_now(&rcutils_now);
-    }
-    if (ret != RCUTILS_RET_OK) {
-      rclcpp::exceptions::throw_from_rcl_error(
-        ret, "Could not get current time: ");
-    }
+  RCLCPP_PUBLIC
+  static
+  Time
+  now(rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
 
-    return Time(std::move(rcutils_now));
-  }
+  RCLCPP_PUBLIC
+  Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_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_));
-    msg_time.nanosec = static_cast<std::uint32_t>(rcl_time_ % (1000 * 1000 * 1000));
-    return msg_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_point_value_t rcl_time_;
-
-  explicit Time(rcl_time_point_value_t && rcl_time)
-  : rcl_time_(std::forward<decltype(rcl_time)>(rcl_time))
-  {}
+  rcl_time_source_t rcl_time_source_;
+  rcl_time_point_t rcl_time_;
 };
 
 }  // namespace rclcpp

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.2</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>

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
 {
@@ -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/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/node_interfaces/node_base.cpp

@@ -92,6 +92,8 @@ NodeBase::NodeBase(
     // 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;

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;
@@ -197,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) ==
@@ -234,8 +238,8 @@ void
 NodeParameters::register_param_change_callback(ParametersCallbackFunction callback)
 {
   if (parameters_callback_) {
-    fprintf(stderr, "Warning: param_change_callback already registered, "
-      "overwriting previous callback\n");
+    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

@@ -125,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
 {

rclcpp/src/rclcpp/service.cpp

@@ -46,13 +46,25 @@ 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

@@ -79,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/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_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_time.cpp

@@ -14,6 +14,8 @@
 
 #include <gtest/gtest.h>
 
+#include <algorithm>
+#include <limits>
 #include <string>
 
 #include "rcl/error_handling.h"
@@ -30,16 +32,16 @@ protected:
   }
 };
 
-TEST(TestTime, rate_basics) {
+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>());
+  EXPECT_ANY_THROW(rclcpp::Time::now(RCL_ROS_TIME));
 
-  Time system_now = rclcpp::Time::now<RCL_SYSTEM_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>();
+  Time steady_now = rclcpp::Time::now(RCL_STEADY_TIME);
   EXPECT_NE(0, steady_now.sec);
   EXPECT_NE(0u, steady_now.nanosec);
 
@@ -48,3 +50,95 @@ TEST(TestTime, rate_basics) {
   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/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -371,53 +371,83 @@ public:
   const State &
   trigger_transition(const Transition & transition);
 
+  RCLCPP_LIFECYCLE_PUBLIC
+  const State &
+  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
-  bool
-  register_on_configure(std::function<rcl_lifecycle_ret_t(const State &)> fcn);
+  const State &
+  shutdown(rcl_lifecycle_transition_key_t & cb_return_code);
 
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_cleanup(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_shutdown(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_activate(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_deactivate(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_error(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_transition_key_t(const State &)> fcn);
 
 protected:
   RCLCPP_LIFECYCLE_PUBLIC

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/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.2</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>

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -245,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 &
@@ -305,12 +316,26 @@ LifecycleNode::trigger_transition(const Transition & transition)
   return trigger_transition(transition.id());
 }
 
+const State &
+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()
 {
@@ -318,6 +343,13 @@ LifecycleNode::configure()
     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()
 {
@@ -325,6 +357,13 @@ LifecycleNode::cleanup()
     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()
 {
@@ -332,6 +371,13 @@ LifecycleNode::activate()
     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()
 {
@@ -339,6 +385,13 @@ LifecycleNode::deactivate()
     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()
 {
@@ -346,6 +399,13 @@ LifecycleNode::shutdown()
     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

@@ -40,6 +40,8 @@
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 
+#include "rcutils/logging_macros.h"
+
 namespace rclcpp_lifecycle
 {
 
@@ -162,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;
@@ -179,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
@@ -226,7 +235,6 @@ 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 (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) {
@@ -273,13 +281,13 @@ 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
@@ -287,56 +295,54 @@ public:
 
     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
         if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) {
-          fprintf(stderr, "Failed to call cleanup on error state\n");
-          return false;
+          RCUTILS_LOG_ERROR("Failed to call cleanup on error state")
+          return RCL_RET_ERROR;
         }
-        // fprintf(stderr, "current state after error callback%s\n",
-        //  state_machine_.current_state->label);
       } else {
         // We call shutdown on the error state
         if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) {
-          fprintf(stderr, "Failed to call cleanup on error state\n");
-          return false;
+          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()) {
@@ -351,7 +357,7 @@ 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;
@@ -360,7 +366,16 @@ public:
   const State &
   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();
   }
 
@@ -380,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/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,57 +66,65 @@ 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) {
@@ -144,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;
   }
 };