0.6.2

* use signal safe synchronization with platform specific semaphores

Signed-off-by: William Woodall <william@osrfoundation.org>

* addressed feedback and refactored into separate files

Signed-off-by: William Woodall <william@osrfoundation.org>

* Apply suggestions from code review

Co-Authored-By: wjwwood <william+github@osrfoundation.org>

* include what you use (cpplint)

Signed-off-by: William Woodall <william@osrfoundation.org>

* avoid redundant use of SignalHandler::

Signed-off-by: William Woodall <william@osrfoundation.org>

* Update rclcpp/src/rclcpp/signal_handler.hpp

Co-Authored-By: wjwwood <william+github@osrfoundation.org>

* fix Windows build

Signed-off-by: William Woodall <william@osrfoundation.org>

* actually fix Windows

Signed-off-by: William Woodall <william@osrfoundation.org>

.gitignore

@@ -0,0 +1 @@
+.DS_Store

rclcpp/CHANGELOG.rst

@@ -2,6 +2,57 @@
 Changelog for package rclcpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.6.2 (2018-12-13)
+------------------
+* Updated to use signal safe synchronization with platform specific semaphores (`#607 <https://github.com/ros2/rclcpp/issues/607>`_)
+* Resolved startup race condition for sim time (`#608 <https://github.com/ros2/rclcpp/issues/608>`_)
+  Resolves `#595 <https://github.com/ros2/rclcpp/issues/595>`_
+* Contributors: Tully Foote, William Woodall
+
+0.6.1 (2018-12-07)
+------------------
+* Added wait_for_action_server() for action clients (`#598 <https://github.com/ros2/rclcpp/issues/598>`_)
+* Added node path and time stamp to parameter event message (`#584 <https://github.com/ros2/rclcpp/issues/584>`_)
+* Updated to allow removing a waitable (`#597 <https://github.com/ros2/rclcpp/issues/597>`_)
+* Refactored init to allow for non-global init (`#587 <https://github.com/ros2/rclcpp/issues/587>`_)
+* Fixed wrong use of constructor and hanging test (`#596 <https://github.com/ros2/rclcpp/issues/596>`_)
+* Added class Waitable (`#589 <https://github.com/ros2/rclcpp/issues/589>`_)
+* Updated rcl_wait_set_add\_* calls (`#586 <https://github.com/ros2/rclcpp/issues/586>`_)
+* Contributors: Dirk Thomas, Jacob Perron, Shane Loretz, William Woodall, bpwilcox
+
+0.6.0 (2018-11-19)
+------------------
+* Updated to use new error handling API from rcutils (`#577 <https://github.com/ros2/rclcpp/issues/577>`_)
+* Added a warning when publishing if publisher is not active (`#574 <https://github.com/ros2/rclcpp/issues/574>`_)
+* Added logging macro signature that accepts std::string (`#573 <https://github.com/ros2/rclcpp/issues/573>`_)
+* Added virtual destructors to classes with virtual functions. (`#566 <https://github.com/ros2/rclcpp/issues/566>`_)
+* Added semicolons to all RCLCPP and RCUTILS macros. (`#565 <https://github.com/ros2/rclcpp/issues/565>`_)
+* Removed std::binary_function usage (`#561 <https://github.com/ros2/rclcpp/issues/561>`_)
+* Updated to avoid auto-activating ROS time if clock topic is being published (`#559 <https://github.com/ros2/rclcpp/issues/559>`_)
+* Fixed cpplint on xenial (`#556 <https://github.com/ros2/rclcpp/issues/556>`_)
+* Added get_parameter_or_set_default. (`#551 <https://github.com/ros2/rclcpp/issues/551>`_)
+* Added max_duration to spin_some() (`#558 <https://github.com/ros2/rclcpp/issues/558>`_)
+* Updated to output rcl error message when yaml parsing fails (`#557 <https://github.com/ros2/rclcpp/issues/557>`_)
+* Updated to make sure timer is fini'd before clock (`#553 <https://github.com/ros2/rclcpp/issues/553>`_)
+* Get node names and namespaces (`#545 <https://github.com/ros2/rclcpp/issues/545>`_)
+* Fixed and improved documentation  (`#546 <https://github.com/ros2/rclcpp/issues/546>`_)
+* Updated to use rcl_clock_t jump callbacks (`#543 <https://github.com/ros2/rclcpp/issues/543>`_)
+* Updated to use rcl consolidated wait set functions (`#540 <https://github.com/ros2/rclcpp/issues/540>`_)
+* Addeed TIME_MAX and DURATION_MAX functions (`#538 <https://github.com/ros2/rclcpp/issues/538>`_)
+* Updated to publish shared_ptr of rcl_serialized_message (`#541 <https://github.com/ros2/rclcpp/issues/541>`_)
+* Added Time::is_zero and Duration::seconds (`#536 <https://github.com/ros2/rclcpp/issues/536>`_)
+* Changed to log an error message instead of throwing exception in destructor (`#535 <https://github.com/ros2/rclcpp/issues/535>`_)
+* Updated to relax tolerance of now test because timing affected by OS scheduling (`#533 <https://github.com/ros2/rclcpp/issues/533>`_)
+* Removed incorrect exception on sec < 0 (`#527 <https://github.com/ros2/rclcpp/issues/527>`_)
+* Added rclcpp::Time::seconds() (`#526 <https://github.com/ros2/rclcpp/issues/526>`_)
+* Updated Timer API to construct TimerBase/GenericTimer with Clock (`#523 <https://github.com/ros2/rclcpp/issues/523>`_)
+* Added rclcpp::is_initialized() (`#522 <https://github.com/ros2/rclcpp/issues/522>`_)
+* Added support for jump handlers with only pre- or post-jump callback (`#517 <https://github.com/ros2/rclcpp/issues/517>`_)
+* Removed use of uninitialized CMake var (`#512 <https://github.com/ros2/rclcpp/issues/512>`_)
+* Updated for Uncrustify 0.67 (`#510 <https://github.com/ros2/rclcpp/issues/510>`_)
+* Added get_node_names API from node. (`#508 <https://github.com/ros2/rclcpp/issues/508>`_)
+* Contributors: Anis Ladram, Chris Lalancette, Dirk Thomas, Francisco Martín Rico, Karsten Knese, Michael Carroll, Mikael Arguedas, Sagnik Basu, Shane Loretz, Sriram Raghunathan, William Woodall, chapulina, dhood
+
 0.5.0 (2018-06-25)
 ------------------
 * Fixed a bug in the multi-threaded executor which could cause it to take a timer (potentially other types of wait-able items) more than once to be worked one. (`#383 <https://github.com/ros2/rclcpp/issues/383>`_)

rclcpp/CMakeLists.txt

@@ -19,12 +19,7 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  # 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")
+  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
 include_directories(include)
@@ -45,6 +40,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/executors/multi_threaded_executor.cpp
   src/rclcpp/executors/single_threaded_executor.cpp
   src/rclcpp/graph_listener.cpp
+  src/rclcpp/init_options.cpp
   src/rclcpp/intra_process_manager.cpp
   src/rclcpp/intra_process_manager_impl.cpp
   src/rclcpp/logger.cpp
@@ -57,8 +53,10 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/node_interfaces/node_logging.cpp
   src/rclcpp/node_interfaces/node_parameters.cpp
   src/rclcpp/node_interfaces/node_services.cpp
+  src/rclcpp/node_interfaces/node_time_source.cpp
   src/rclcpp/node_interfaces/node_timers.cpp
   src/rclcpp/node_interfaces/node_topics.cpp
+  src/rclcpp/node_interfaces/node_waitables.cpp
   src/rclcpp/parameter.cpp
   src/rclcpp/parameter_value.cpp
   src/rclcpp/parameter_client.cpp
@@ -67,12 +65,14 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/parameter_service.cpp
   src/rclcpp/publisher.cpp
   src/rclcpp/service.cpp
+  src/rclcpp/signal_handler.cpp
   src/rclcpp/subscription.cpp
   src/rclcpp/time.cpp
   src/rclcpp/time_source.cpp
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
   src/rclcpp/utilities.cpp
+  src/rclcpp/waitable.cpp
 )
 
 # "watch" template for changes
@@ -247,8 +247,7 @@ if(BUILD_TESTING)
     )
     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})
+  ament_add_gtest(test_rate test/test_rate.cpp)
   if(TARGET test_rate)
     target_include_directories(test_rate PUBLIC
       ${rcl_interfaces_INCLUDE_DIRS}
@@ -260,8 +259,7 @@ if(BUILD_TESTING)
       ${PROJECT_NAME}
     )
   endif()
-  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp
-    ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation})
+  ament_add_gtest(test_serialized_message_allocator test/test_serialized_message_allocator.cpp)
   if(TARGET test_serialized_message_allocator)
     target_include_directories(test_serialized_message_allocator PUBLIC
       ${test_msgs_INCLUDE_DIRS}
@@ -388,6 +386,14 @@ if(BUILD_TESTING)
     target_link_libraries(test_utilities ${PROJECT_NAME})
   endif()
 
+  ament_add_gtest(test_init test/test_init.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_init)
+    ament_target_dependencies(test_init
+      "rcl")
+    target_link_libraries(test_init ${PROJECT_NAME})
+  endif()
+
   ament_add_gtest(test_multi_threaded_executor test/executors/test_multi_threaded_executor.cpp
     APPEND_LIBRARY_DIRS "${append_library_dirs}")
   if(TARGET test_multi_threaded_executor)

rclcpp/include/rclcpp/any_executable.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -45,6 +46,7 @@ struct AnyExecutable
   rclcpp::TimerBase::SharedPtr timer;
   rclcpp::ServiceBase::SharedPtr service;
   rclcpp::ClientBase::SharedPtr client;
+  rclcpp::Waitable::SharedPtr waitable;
   // These are used to keep the scope on the containing items
   rclcpp::callback_group::CallbackGroup::SharedPtr callback_group;
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base;

rclcpp/include/rclcpp/any_service_callback.hpp

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

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -38,15 +38,15 @@ class AnySubscriptionCallback
   using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
   using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
 
-  using SharedPtrCallback = std::function<void(const std::shared_ptr<MessageT>)>;
+  using SharedPtrCallback = std::function<void (const std::shared_ptr<MessageT>)>;
   using SharedPtrWithInfoCallback =
-    std::function<void(const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
-  using ConstSharedPtrCallback = std::function<void(const std::shared_ptr<const MessageT>)>;
+    std::function<void (const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
+  using ConstSharedPtrCallback = std::function<void (const std::shared_ptr<const MessageT>)>;
   using ConstSharedPtrWithInfoCallback =
-    std::function<void(const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
-  using UniquePtrCallback = std::function<void(MessageUniquePtr)>;
+    std::function<void (const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
+  using UniquePtrCallback = std::function<void (MessageUniquePtr)>;
   using UniquePtrWithInfoCallback =
-    std::function<void(MessageUniquePtr, const rmw_message_info_t &)>;
+    std::function<void (MessageUniquePtr, const rmw_message_info_t &)>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback_;

rclcpp/include/rclcpp/callback_group.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -35,6 +36,7 @@ namespace node_interfaces
 class NodeServices;
 class NodeTimers;
 class NodeTopics;
+class NodeWaitables;
 }  // namespace node_interfaces
 
 namespace callback_group
@@ -51,6 +53,7 @@ class CallbackGroup
   friend class rclcpp::node_interfaces::NodeServices;
   friend class rclcpp::node_interfaces::NodeTimers;
   friend class rclcpp::node_interfaces::NodeTopics;
+  friend class rclcpp::node_interfaces::NodeWaitables;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup)
@@ -74,6 +77,10 @@ public:
   const std::vector<rclcpp::ClientBase::WeakPtr> &
   get_client_ptrs() const;
 
+  RCLCPP_PUBLIC
+  const std::vector<rclcpp::Waitable::WeakPtr> &
+  get_waitable_ptrs() const;
+
   RCLCPP_PUBLIC
   std::atomic_bool &
   can_be_taken_from();
@@ -101,6 +108,14 @@ protected:
   void
   add_client(const rclcpp::ClientBase::SharedPtr client_ptr);
 
+  RCLCPP_PUBLIC
+  void
+  add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr);
+
+  RCLCPP_PUBLIC
+  void
+  remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept;
+
   CallbackGroupType type_;
   // Mutex to protect the subsequent vectors of pointers.
   mutable std::mutex mutex_;
@@ -108,6 +123,7 @@ protected:
   std::vector<rclcpp::TimerBase::WeakPtr> timer_ptrs_;
   std::vector<rclcpp::ServiceBase::WeakPtr> service_ptrs_;
   std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
+  std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
   std::atomic_bool can_be_taken_from_;
 };
 

rclcpp/include/rclcpp/client.hpp

@@ -110,6 +110,7 @@ protected:
 
   rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
   std::shared_ptr<rcl_node_t> node_handle_;
+  std::shared_ptr<rclcpp::Context> context_;
 
   std::shared_ptr<rcl_client_t> client_handle_;
 };
@@ -130,8 +131,8 @@ public:
   using SharedFuture = std::shared_future<SharedResponse>;
   using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
 
-  using CallbackType = std::function<void(SharedFuture)>;
-  using CallbackWithRequestType = std::function<void(SharedFutureWithRequest)>;
+  using CallbackType = std::function<void (SharedFuture)>;
+  using CallbackWithRequestType = std::function<void (SharedFutureWithRequest)>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Client)
 
@@ -171,13 +172,13 @@ public:
   }
 
   std::shared_ptr<void>
-  create_response()
+  create_response() override
   {
     return std::shared_ptr<void>(new typename ServiceT::Response());
   }
 
   std::shared_ptr<rmw_request_id_t>
-  create_request_header()
+  create_request_header() override
   {
     // TODO(wjwwood): This should probably use rmw_request_id's allocator.
     //                (since it is a C type)
@@ -187,7 +188,7 @@ public:
   void
   handle_response(
     std::shared_ptr<rmw_request_id_t> request_header,
-    std::shared_ptr<void> response)
+    std::shared_ptr<void> response) override
   {
     std::unique_lock<std::mutex> lock(pending_requests_mutex_);
     auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);

rclcpp/include/rclcpp/clock.hpp

@@ -31,53 +31,18 @@ namespace rclcpp
 
 class TimeSource;
 
-/// A struct to represent a timejump.
-/**
- * It represents the time jump duration and whether it changed clock type.
- */
-struct TimeJump
-{
-  typedef enum ClockChange_t
-  {
-    ROS_TIME_NO_CHANGE,    ///< The time type before and after the jump is ROS_TIME
-    ROS_TIME_ACTIVATED,    ///< The time type switched to ROS_TIME from SYSTEM_TIME
-    ROS_TIME_DEACTIVATED,  ///< The time type switched to SYSTEM_TIME from ROS_TIME
-    SYSTEM_TIME_NO_CHANGE  ///< The time type before and after the jump is SYSTEM_TIME
-  } ClockChange_t;
-
-  ClockChange_t jump_type_;  ///< The change in clock_type if there is one.
-  rcl_duration_t delta_;     ///< The change in time value.
-};
-
-/// A class to store a threshold for a TimeJump.
-/**
- * This class can be used to evaluate a time jump's magnitude.
- */
-class JumpThreshold
-{
-public:
-  uint64_t min_forward_;   ///< The minimum jump forward to be considered exceeded..
-  uint64_t min_backward_;  ///< The minimum backwards jump to be considered exceeded.
-  bool on_clock_change_;   ///< Whether to trigger on any clock type change.
-
-  // Test if the threshold is exceeded by a TimeJump
-  RCLCPP_PUBLIC
-  bool
-  is_exceeded(const TimeJump & jump);
-};
-
 class JumpHandler
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler)
   JumpHandler(
     std::function<void()> pre_callback,
-    std::function<void(TimeJump)> post_callback,
-    JumpThreshold & threshold);
+    std::function<void(const rcl_time_jump_t &)> post_callback,
+    const rcl_jump_threshold_t & threshold);
 
   std::function<void()> pre_callback;
-  std::function<void(const TimeJump &)> post_callback;
-  JumpThreshold notice_threshold;
+  std::function<void(const rcl_time_jump_t &)> post_callback;
+  rcl_jump_threshold_t notice_threshold;
 };
 
 class Clock
@@ -99,6 +64,10 @@ public:
   bool
   ros_time_is_active();
 
+  RCLCPP_PUBLIC
+  rcl_clock_t *
+  get_clock_handle();
+
   RCLCPP_PUBLIC
   rcl_clock_type_t
   get_clock_type();
@@ -112,32 +81,22 @@ public:
   JumpHandler::SharedPtr
   create_jump_callback(
     std::function<void()> pre_callback,
-    std::function<void(const TimeJump &)> post_callback,
-    JumpThreshold & threshold);
+    std::function<void(const rcl_time_jump_t &)> post_callback,
+    const rcl_jump_threshold_t & threshold);
 
 private:
-  // A method for TimeSource to get a list of callbacks to invoke while updating
+  // Invoke time jump callback
   RCLCPP_PUBLIC
-  std::vector<JumpHandler::SharedPtr>
-  get_triggered_callback_handlers(const TimeJump & jump);
-
-  // Invoke callbacks that are valid and outside threshold.
-  RCLCPP_PUBLIC
-  static void invoke_prejump_callbacks(
-    const std::vector<JumpHandler::SharedPtr> & callbacks);
-
-  RCLCPP_PUBLIC
-  static void invoke_postjump_callbacks(
-    const std::vector<JumpHandler::SharedPtr> & callbacks,
-    const TimeJump & jump);
+  static void
+  on_time_jump(
+    const struct rcl_time_jump_t * time_jump,
+    bool before_jump,
+    void * user_data);
 
   /// Internal storage backed by rcl
   rcl_clock_t rcl_clock_;
   friend TimeSource;  /// Allow TimeSource to access the rcl_clock_ datatype.
   rcl_allocator_t allocator_;
-
-  std::mutex callback_list_mutex_;
-  std::vector<std::weak_ptr<JumpHandler>> active_jump_handlers_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/context.hpp

@@ -15,34 +15,292 @@
 #ifndef RCLCPP__CONTEXT_HPP_
 #define RCLCPP__CONTEXT_HPP_
 
-#include <iostream>
+#include <condition_variable>
+#include <functional>
 #include <memory>
 #include <mutex>
+#include <string>
 #include <typeindex>
 #include <typeinfo>
 #include <unordered_map>
 #include <utility>
+#include <vector>
 
+#include "rcl/context.h"
+#include "rcl/guard_condition.h"
+#include "rcl/wait.h"
+#include "rclcpp/init_options.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
-#include "rmw/rmw.h"
 
 namespace rclcpp
 {
 
-class Context
+/// Thrown when init is called on an already initialized context.
+class ContextAlreadyInitialized : public std::runtime_error
+{
+public:
+  ContextAlreadyInitialized()
+  : std::runtime_error("context is already initialized") {}
+};
+
+/// Context which encapsulates shared state between nodes and other similar entities.
+/**
+ * A context also represents the lifecycle between init and shutdown of rclcpp.
+ * It is often used in conjunction with rclcpp::init, or rclcpp::init_local,
+ * and rclcpp::shutdown.
+ */
+class Context : public std::enable_shared_from_this<Context>
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(Context)
 
+  /// Default constructor, after which the Context is still not "initialized".
+  /**
+   * Every context which is constructed is added to a global vector of contexts,
+   * which is used by the signal handler to conditionally shutdown each context
+   * on SIGINT.
+   * See the shutdown_on_sigint option in the InitOptions class.
+   */
   RCLCPP_PUBLIC
   Context();
 
+  RCLCPP_PUBLIC
+  virtual
+  ~Context();
+
+  /// Initialize the context, and the underlying elements like the rcl context.
+  /**
+   * This method must be called before passing this context to things like the
+   * constructor of Node.
+   * It must also be called before trying to shutdown the context.
+   *
+   * Note that this function does not setup any signal handlers, so if you want
+   * it to be shutdown by the signal handler, then you need to either install
+   * them manually with rclcpp::install_signal_handers() or use rclcpp::init().
+   * In addition to installing the signal handlers, the shutdown_on_sigint
+   * of the InitOptions needs to be `true` for this context to be shutdown by
+   * the signal handler, otherwise it will be passed over.
+   *
+   * After calling this method, shutdown() can be called to invalidate the
+   * context for derived entities, e.g. nodes, guard conditions, etc.
+   * However, the underlying rcl context is not finalized until this Context's
+   * destructor is called or this function is called again.
+   * Allowing this class to go out of scope and get destructed or calling this
+   * function a second time while derived entities are still using the context
+   * is undefined behavior and should be avoided.
+   * It's a good idea to not reuse context objects and instead create a new one
+   * each time you need to shutdown and init again.
+   * This allows derived entities to hold on to shard pointers to the first
+   * context object until they are done.
+   *
+   * This function is thread-safe.
+   *
+   * \param[in] argc number of arguments
+   * \param[in] argv argument array which may contain arguments intended for ROS
+   * \param[in] init_options initialization options for rclcpp and underlying layers
+   * \throw ContextAlreadyInitialized if called if init is called more than once
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  init(
+    int argc,
+    char const * const argv[],
+    const rclcpp::InitOptions & init_options = rclcpp::InitOptions());
+
+  /// Return true if the context is valid, otherwise false.
+  /**
+   * The context is valid if it has been initialized but not shutdown.
+   *
+   * This function is thread-safe.
+   * This function is lock free so long as pointers and uint64_t atomics are
+   * lock free.
+   *
+   * \return true if valid, otherwise false
+   */
+  RCLCPP_PUBLIC
+  bool
+  is_valid() const;
+
+  /// Return the init options used during init.
+  RCLCPP_PUBLIC
+  const rclcpp::InitOptions &
+  get_init_options() const;
+
+  /// Return a copy of the init options used during init.
+  RCLCPP_PUBLIC
+  rclcpp::InitOptions
+  get_init_options();
+
+  /// Return the shutdown reason, or empty string if not shutdown.
+  /**
+   * This function is thread-safe.
+   */
+  RCLCPP_PUBLIC
+  std::string
+  shutdown_reason();
+
+  /// Shutdown the context, making it uninitialized and therefore invalid for derived entities.
+  /**
+   * Several things happen when the context is shutdown, in this order:
+   *
+   * - acquires a lock to prevent race conditions with init, on_shutdown, etc.
+   * - if the context is not initialized, return false
+   * - rcl_shutdown() is called on the internal rcl_context_t instance
+   * - the shutdown reason is set
+   * - each on_shutdown callback is called, in the order that they were added
+   * - interrupt blocking sleep_for() calls, so they return early due to shutdown
+   * - interrupt blocking executors and wait sets
+   *
+   * The underlying rcl context is not finalized by this function.
+   *
+   * This function is thread-safe.
+   *
+   * \param[in] reason the description of why shutdown happened
+   * \return true if shutdown was successful, false if context was already shutdown
+   * \throw various exceptions derived from RCLErrorBase, if rcl_shutdown fails
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  shutdown(const std::string & reason);
+
+  using OnShutdownCallback = std::function<void ()>;
+
+  /// Add a on_shutdown callback to be called when shutdown is called for this context.
+  /**
+   * These callbacks will be called in the order they are added as the second
+   * to last step in shutdown().
+   *
+   * When shutdown occurs due to the signal handler, these callbacks are run
+   * asynchronoulsy in the dedicated singal handling thread.
+   *
+   * Also, shutdown() may be called from the destructor of this function.
+   * Therefore, it is not safe to throw exceptions from these callbacks.
+   * Instead, log errors or use some other mechanism to indicate an error has
+   * occurred.
+   *
+   * On shutdown callbacks may be registered before init and after shutdown,
+   * and persist on repeated init's.
+   *
+   * \param[in] callback the on shutdown callback to be registered
+   * \return the callback passed, for convenience when storing a passed lambda
+   */
+  RCLCPP_PUBLIC
+  virtual
+  OnShutdownCallback
+  on_shutdown(OnShutdownCallback callback);
+
+  /// Return the shutdown callbacks as const.
+  /**
+   * Using the returned reference is not thread-safe with calls that modify
+   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   */
+  RCLCPP_PUBLIC
+  const std::vector<OnShutdownCallback> &
+  get_on_shutdown_callbacks() const;
+
+  /// Return the shutdown callbacks.
+  /**
+   * Using the returned reference is not thread-safe with calls that modify
+   * the list of "on shutdown" callbacks, i.e. on_shutdown().
+   */
+  RCLCPP_PUBLIC
+  std::vector<OnShutdownCallback> &
+  get_on_shutdown_callbacks();
+
+  /// Return the internal rcl context.
+  RCLCPP_PUBLIC
+  std::shared_ptr<rcl_context_t>
+  get_rcl_context();
+
+  /// Sleep for a given period of time or until shutdown() is called.
+  /**
+   * This function can be interrupted early if:
+   *
+   *   - this context is shutdown()
+   *   - this context is destructed (resulting in shutdown)
+   *   - this context has shutdown_on_sigint=true and SIGINT occurs (resulting in shutdown)
+   *   - interrupt_all_sleep_for() is called
+   *
+   * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
+   * \return true if the condition variable did not timeout, i.e. you were interrupted.
+   */
+  RCLCPP_PUBLIC
+  bool
+  sleep_for(const std::chrono::nanoseconds & nanoseconds);
+
+  /// Interrupt any blocking sleep_for calls, causing them to return immediately and return true.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  interrupt_all_sleep_for();
+
+  /// Get a handle to the guard condition which is triggered when interrupted.
+  /**
+   * This guard condition is triggered any time interrupt_all_wait_sets() is
+   * called, which may be called by the user, or shutdown().
+   * And in turn, shutdown() may be called by the user, the destructor of this
+   * context, or the signal handler if installed and shutdown_on_sigint is true
+   * for this context.
+   *
+   * The first time that this function is called for a given wait set a new guard
+   * condition will be created and returned; thereafter the same guard condition
+   * will be returned for the same wait set.
+   * This mechanism is designed to ensure that the same guard condition is not
+   * reused across wait sets (e.g., when using multiple executors in the same
+   * process).
+   * This method will throw an exception if initialization of the guard
+   * condition fails.
+   *
+   * The returned guard condition needs to be released with the
+   * release_interrupt_guard_condition() method in order to reclaim resources.
+   *
+   * \param[in] wait_set Pointer to the rcl_wait_set_t that will be using the
+   *   resulting guard condition.
+   * \return Pointer to the guard condition.
+   */
+  RCLCPP_PUBLIC
+  rcl_guard_condition_t *
+  get_interrupt_guard_condition(rcl_wait_set_t * wait_set);
+
+  /// Release the previously allocated guard condition which is triggered when interrupted.
+  /**
+   * If you previously called get_interrupt_guard_condition() for a given wait
+   * set to get a interrupt guard condition, then you should call
+   * release_interrupt_guard_condition() when you're done, to free that
+   * condition.
+   * Will throw an exception if get_interrupt_guard_condition() wasn't
+   * previously called for the given wait set.
+   *
+   * After calling this, the pointer returned by get_interrupt_guard_condition()
+   * for the given wait_set is invalid.
+   *
+   * \param[in] wait_set Pointer to the rcl_wait_set_t that was using the
+   *   resulting guard condition.
+   */
+  RCLCPP_PUBLIC
+  void
+  release_interrupt_guard_condition(rcl_wait_set_t * wait_set);
+
+  /// Nothrow version of release_interrupt_guard_condition(), logs to RCLCPP_ERROR instead.
+  RCLCPP_PUBLIC
+  void
+  release_interrupt_guard_condition(rcl_wait_set_t * wait_set, const std::nothrow_t &) noexcept;
+
+  /// Interrupt any blocking executors, or wait sets associated with this context.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  interrupt_all_wait_sets();
+
+  /// Return a singleton instance for the SubContext type, constructing one if necessary.
   template<typename SubContext, typename ... Args>
   std::shared_ptr<SubContext>
   get_sub_context(Args && ... args)
   {
-    std::lock_guard<std::mutex> lock(mutex_);
+    std::lock_guard<std::recursive_mutex> lock(sub_contexts_mutex_);
 
     std::type_index type_i(typeid(SubContext));
     std::shared_ptr<SubContext> sub_context;
@@ -62,13 +320,51 @@ public:
     return sub_context;
   }
 
+protected:
+  // Called by constructor and destructor to clean up by finalizing the
+  // shutdown rcl context and preparing for a new init cycle.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  clean_up();
+
 private:
   RCLCPP_DISABLE_COPY(Context)
 
+  // This mutex is recursive so that the destructor can ensure atomicity
+  // between is_initialized and shutdown.
+  std::recursive_mutex init_mutex_;
+  std::shared_ptr<rcl_context_t> rcl_context_;
+  rclcpp::InitOptions init_options_;
+  std::string shutdown_reason_;
+
   std::unordered_map<std::type_index, std::shared_ptr<void>> sub_contexts_;
-  std::mutex mutex_;
+  // This mutex is recursive so that the constructor of a sub context may
+  // attempt to acquire another sub context.
+  std::recursive_mutex sub_contexts_mutex_;
+
+  std::vector<OnShutdownCallback> on_shutdown_callbacks_;
+  std::mutex on_shutdown_callbacks_mutex_;
+
+  /// Condition variable for timed sleep (see sleep_for).
+  std::condition_variable interrupt_condition_variable_;
+  /// Mutex for protecting the global condition variable.
+  std::mutex interrupt_mutex_;
+
+  /// Mutex to protect sigint_guard_cond_handles_.
+  std::mutex interrupt_guard_cond_handles_mutex_;
+  /// Guard conditions for interrupting of associated wait sets on interrupt_all_wait_sets().
+  std::unordered_map<rcl_wait_set_t *, rcl_guard_condition_t> interrupt_guard_cond_handles_;
 };
 
+/// Return a copy of the list of context shared pointers.
+/**
+ * This function is thread-safe.
+ */
+RCLCPP_PUBLIC
+std::vector<Context::SharedPtr>
+get_contexts();
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CONTEXT_HPP_

rclcpp/include/rclcpp/duration.hpp

@@ -89,6 +89,10 @@ public:
   Duration
   operator-(const rclcpp::Duration & rhs) const;
 
+  RCLCPP_PUBLIC
+  static Duration
+  max();
+
   RCLCPP_PUBLIC
   Duration
   operator*(double scale) const;
@@ -97,6 +101,13 @@ public:
   rcl_duration_value_t
   nanoseconds() const;
 
+  /// \return the duration in seconds as a floating point number.
+  /// \warning Depending on sizeof(double) there could be significant precision loss.
+  /// When an exact time is required use nanoseconds() instead.
+  RCLCPP_PUBLIC
+  double
+  seconds() const;
+
 private:
   rcl_duration_t rcl_duration_;
 };

rclcpp/include/rclcpp/exceptions.hpp

@@ -116,7 +116,7 @@ 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);
+  void (* reset_error)() = rcl_reset_error);
 
 class RCLErrorBase
 {

rclcpp/include/rclcpp/executor.hpp

@@ -28,9 +28,10 @@
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
 
-#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/memory_strategies.hpp"
 #include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
 
@@ -65,16 +66,20 @@ to_string(const FutureReturnCode & future_return_code);
  */
 struct ExecutorArgs
 {
+  ExecutorArgs()
+  : memory_strategy(memory_strategies::create_default_strategy()),
+    context(rclcpp::contexts::default_context::get_global_default_context()),
+    max_conditions(0)
+  {}
+
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy;
-  size_t max_conditions = 0;
+  std::shared_ptr<rclcpp::Context> context;
+  size_t max_conditions;
 };
 
 static inline ExecutorArgs create_default_executor_arguments()
 {
-  ExecutorArgs args;
-  args.memory_strategy = memory_strategies::create_default_strategy();
-  args.max_conditions = 0;
-  return args;
+  return ExecutorArgs();
 }
 
 /// Coordinate the order and timing of available communication tasks.
@@ -95,7 +100,7 @@ public:
   /// Default constructor.
   // \param[in] ms The memory strategy to be used with this executor.
   RCLCPP_PUBLIC
-  explicit Executor(const ExecutorArgs & args = create_default_executor_arguments());
+  explicit Executor(const ExecutorArgs & args = ExecutorArgs());
 
   /// Default destructor.
   RCLCPP_PUBLIC
@@ -190,10 +195,14 @@ public:
    * single-threaded model of execution.
    * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
    * to block (which may have unintended consequences).
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
+   * Note that spin_some() may take longer than this time as it only returns once max_duration has
+   * been exceeded.
    */
   RCLCPP_PUBLIC
   virtual void
-  spin_some();
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
 
   RCLCPP_PUBLIC
   virtual void
@@ -233,7 +242,7 @@ public:
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
-    while (rclcpp::ok()) {
+    while (rclcpp::ok(this->context_)) {
       // Do one item of work.
       spin_once(timeout_left);
       // Check if the future is set, return SUCCESS if it is.
@@ -349,6 +358,9 @@ protected:
   /// The memory strategy: an interface for handling user-defined memory allocation strategies.
   memory_strategy::MemoryStrategy::SharedPtr memory_strategy_;
 
+  /// The context associated with this executor.
+  std::shared_ptr<rclcpp::Context> context_;
+
 private:
   RCLCPP_DISABLE_COPY(Executor)
 

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -45,11 +45,13 @@ public:
    * once.
    *
    * \param args common arguments for all executors
+   * \param number_of_threads number of threads to have in the thread pool,
+   *   the default 0 will use the number of cpu cores found instead
    * \param yield_before_execute if true std::this_thread::yield() is called
    */
   RCLCPP_PUBLIC
   MultiThreadedExecutor(
-    const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments(),
+    const executor::ExecutorArgs & args = executor::ExecutorArgs(),
     size_t number_of_threads = 0,
     bool yield_before_execute = false);
 

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -45,7 +45,7 @@ public:
   /// Default constructor. See the default constructor for Executor.
   RCLCPP_PUBLIC
   SingleThreadedExecutor(
-    const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments());
+    const executor::ExecutorArgs & args = executor::ExecutorArgs());
 
   /// Default destrcutor.
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/function_traits.hpp

@@ -130,20 +130,20 @@ struct function_traits<FunctionT &&>: function_traits<FunctionT>
  */
 template<std::size_t Arity, typename FunctorT>
 struct arity_comparator : std::integral_constant<
-    bool, (Arity == function_traits<FunctorT>::arity)>{};
+    bool, (Arity == function_traits<FunctorT>::arity)> {};
 
 template<typename FunctorT, typename ... Args>
 struct check_arguments : std::is_same<
     typename function_traits<FunctorT>::arguments,
     std::tuple<Args ...>
-  >
+>
 {};
 
 template<typename FunctorAT, typename FunctorBT>
 struct same_arguments : std::is_same<
     typename function_traits<FunctorAT>::arguments,
     typename function_traits<FunctorBT>::arguments
-  >
+>
 {};
 
 }  // namespace function_traits

rclcpp/include/rclcpp/graph_listener.hpp

@@ -23,6 +23,7 @@
 
 #include "rcl/guard_condition.h"
 #include "rcl/wait.h"
+#include "rclcpp/context.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -62,7 +63,7 @@ class GraphListener : public std::enable_shared_from_this<GraphListener>
 {
 public:
   RCLCPP_PUBLIC
-  GraphListener();
+  explicit GraphListener(std::shared_ptr<rclcpp::Context> parent_context);
 
   RCLCPP_PUBLIC
   virtual ~GraphListener();
@@ -133,6 +134,12 @@ public:
   void
   shutdown();
 
+  /// Nothrow version of shutdown(), logs to RCLCPP_ERROR instead.
+  RCLCPP_PUBLIC
+  virtual
+  void
+  shutdown(const std::nothrow_t &) noexcept;
+
   /// Return true if shutdown() has been called, else false.
   RCLCPP_PUBLIC
   virtual
@@ -154,6 +161,12 @@ protected:
 private:
   RCLCPP_DISABLE_COPY(GraphListener)
 
+  /** \internal */
+  void
+  __shutdown(bool);
+
+  rclcpp::Context::SharedPtr parent_context_;
+
   std::thread listener_thread_;
   bool is_started_;
   std::atomic_bool is_shutdown_;
@@ -164,6 +177,7 @@ private:
   std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
 
   rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
+  std::shared_ptr<rcl_context_t> interrupt_guard_condition_context_;
   rcl_guard_condition_t * shutdown_guard_condition_;
   rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
 };

rclcpp/include/rclcpp/init_options.hpp

@@ -0,0 +1,69 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__INIT_OPTIONS_HPP_
+#define RCLCPP__INIT_OPTIONS_HPP_
+
+#include <memory>
+
+#include "rcl/init_options.h"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Encapsulation of options for initializing rclcpp.
+class InitOptions
+{
+public:
+  /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed).
+  bool shutdown_on_sigint = true;
+
+  /// Constructor which allows you to specify the allocator used within the init options.
+  RCLCPP_PUBLIC
+  explicit InitOptions(rcl_allocator_t allocator = rcl_get_default_allocator());
+
+  /// Constructor which is initialized by an existing init_options.
+  RCLCPP_PUBLIC
+  explicit InitOptions(const rcl_init_options_t & init_options);
+
+  /// Copy constructor.
+  RCLCPP_PUBLIC
+  InitOptions(const InitOptions & other);
+
+  /// Assignment operator.
+  RCLCPP_PUBLIC
+  InitOptions &
+  operator=(const InitOptions & other);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~InitOptions();
+
+  /// Return the rcl init options.
+  RCLCPP_PUBLIC
+  const rcl_init_options_t *
+  get_rcl_init_options() const;
+
+protected:
+  void
+  finalize_init_options();
+
+private:
+  std::unique_ptr<rcl_init_options_t> init_options_;
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__INIT_OPTIONS_HPP_

rclcpp/include/rclcpp/intra_process_manager.hpp

@@ -326,7 +326,7 @@ public:
       message_sequence_number,
       requesting_subscriptions_intra_process_id,
       target_subs_size
-      );
+    );
     typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
     if (!typed_buffer) {
       return;

rclcpp/include/rclcpp/intra_process_manager_impl.hpp

@@ -45,7 +45,7 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerImplBase)
 
   IntraProcessManagerImplBase() = default;
-  ~IntraProcessManagerImplBase() = default;
+  virtual ~IntraProcessManagerImplBase() = default;
 
   virtual void
   add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) = 0;
@@ -262,7 +262,7 @@ private:
     std::hash<uint64_t>, std::equal_to<uint64_t>,
     RebindAlloc<std::pair<const uint64_t, SubscriptionBase::WeakPtr>>>;
 
-  struct strcmp_wrapper : public std::binary_function<const char *, const char *, bool>
+  struct strcmp_wrapper
   {
     bool
     operator()(const char * lhs, const char * rhs) const

rclcpp/include/rclcpp/logger.hpp

@@ -20,6 +20,8 @@
 
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcl/node.h"
+
 /**
  * \def RCLCPP_LOGGING_ENABLED
  * When this define evaluates to true (default), logger factory functions will
@@ -60,6 +62,18 @@ RCLCPP_PUBLIC
 Logger
 get_logger(const std::string & name);
 
+/// Return a named logger using an rcl_node_t.
+/**
+ * This is a convenience function that does error checking and returns the node
+ * logger name, or "rclcpp" if it is unable to get the node name.
+ *
+ * \param[in] node the rcl node from which to get the logger name
+ * \return a logger based on the node name, or "rclcpp" if there's an error
+ */
+RCLCPP_PUBLIC
+Logger
+get_node_logger(const rcl_node_t * node);
+
 class Logger
 {
 private:

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -25,6 +25,7 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 namespace rclcpp
 {
@@ -43,6 +44,8 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
   using WeakNodeVector = std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
 
+  virtual ~MemoryStrategy() = default;
+
   virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
 
   virtual size_t number_of_ready_subscriptions() const = 0;
@@ -50,6 +53,7 @@ public:
   virtual size_t number_of_ready_clients() const = 0;
   virtual size_t number_of_ready_timers() const = 0;
   virtual size_t number_of_guard_conditions() const = 0;
+  virtual size_t number_of_waitables() const = 0;
 
   virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0;
   virtual void clear_handles() = 0;
@@ -74,6 +78,11 @@ public:
     rclcpp::executor::AnyExecutable & any_exec,
     const WeakNodeVector & weak_nodes) = 0;
 
+  virtual void
+  get_next_waitable(
+    rclcpp::executor::AnyExecutable & any_exec,
+    const WeakNodeVector & weak_nodes) = 0;
+
   virtual rcl_allocator_t
   get_allocator() = 0;
 
@@ -111,6 +120,11 @@ public:
   get_group_by_client(
     rclcpp::ClientBase::SharedPtr client,
     const WeakNodeVector & weak_nodes);
+
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_waitable(
+    rclcpp::Waitable::SharedPtr waitable,
+    const WeakNodeVector & weak_nodes);
 };
 
 }  // namespace memory_strategy

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -25,6 +25,8 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/visibility_control.hpp"
 
+#include "rcutils/logging_macros.h"
+
 #include "rmw/serialized_message.h"
 
 namespace rclcpp
@@ -69,6 +71,8 @@ public:
     rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
   }
 
+  virtual ~MessageMemoryStrategy() = default;
+
   /// Default factory method
   static SharedPtr create_default()
   {
@@ -96,7 +100,9 @@ public:
           auto ret = rmw_serialized_message_fini(msg);
           delete msg;
           if (ret != RCL_RET_OK) {
-            rclcpp::exceptions::throw_from_rcl_error(ret, "leaking memory");
+            RCUTILS_LOG_ERROR_NAMED(
+              "rclcpp",
+              "failed to destroy serialized message: %s", rcl_get_error_string().str);
           }
         });
 

rclcpp/include/rclcpp/node.hpp

@@ -47,8 +47,10 @@
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
+#include "rclcpp/node_interfaces/node_time_source_interface.hpp"
 #include "rclcpp/node_interfaces/node_timers_interface.hpp"
 #include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
 #include "rclcpp/parameter.hpp"
 #include "rclcpp/publisher.hpp"
 #include "rclcpp/service.hpp"
@@ -91,6 +93,8 @@ public:
    * \param[in] use_global_arguments False to prevent node using arguments passed to the process.
    * \param[in] use_intra_process_comms True to use the optimized intra-process communication
    * pipeline to pass messages between nodes in the same process using shared memory.
+   * \param[in] start_parameter_services True to setup ROS interfaces for accessing parameters
+   * in the node.
    */
   RCLCPP_PUBLIC
   Node(
@@ -319,6 +323,24 @@ public:
     ParameterT & value,
     const ParameterT & alternative_value) const;
 
+  /// Get the parameter value; if not set, set the "alternative value" and store it in the node.
+  /**
+   * If the parameter is set, then the "value" argument is assigned the value
+   * in the parameter.
+   * If the parameter is not set, then the "value" argument is assigned the "alternative_value",
+   * and the parameter is set to the "alternative_value" on the node.
+   *
+   * \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 and parameter if the parameter was not set.
+   */
+  template<typename ParameterT>
+  void
+  get_parameter_or_set(
+    const std::string & name,
+    ParameterT & value,
+    const ParameterT & alternative_value);
+
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::ParameterDescriptor>
   describe_parameters(const std::vector<std::string> & names) const;
@@ -341,6 +363,10 @@ public:
   void
   register_param_change_callback(CallbackT && callback);
 
+  RCLCPP_PUBLIC
+  std::vector<std::string>
+  get_node_names() const;
+
   RCLCPP_PUBLIC
   std::map<std::string, std::vector<std::string>>
   get_topic_names_and_types() const;
@@ -403,6 +429,11 @@ public:
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr
   get_node_graph_interface();
 
+  /// Return the Node's internal NodeLoggingInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+  get_node_logging_interface();
+
   /// Return the Node's internal NodeTimersInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
@@ -418,11 +449,21 @@ public:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr
   get_node_services_interface();
 
+  /// Return the Node's internal NodeWaitablesInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
+  get_node_waitables_interface();
+
   /// Return the Node's internal NodeParametersInterface implementation.
   RCLCPP_PUBLIC
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
   get_node_parameters_interface();
 
+  /// Return the Node's internal NodeParametersInterface implementation.
+  RCLCPP_PUBLIC
+  rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
+  get_node_time_source_interface();
+
 private:
   RCLCPP_DISABLE_COPY(Node)
 
@@ -436,8 +477,10 @@ private:
   rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
-  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_;
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
 
   bool use_intra_process_comms_;
 };

rclcpp/include/rclcpp/node_impl.hpp

@@ -160,7 +160,8 @@ Node::create_wall_timer(
 {
   auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
     std::chrono::duration_cast<std::chrono::nanoseconds>(period),
-    std::move(callback));
+    std::move(callback),
+    this->node_base_->get_context());
   node_timers_->add_timer(timer, group);
   return timer;
 }
@@ -250,6 +251,22 @@ Node::get_parameter_or(
   return got_parameter;
 }
 
+template<typename ParameterT>
+void
+Node::get_parameter_or_set(
+  const std::string & name,
+  ParameterT & value,
+  const ParameterT & alternative_value)
+{
+  bool got_parameter = get_parameter(name, value);
+  if (!got_parameter) {
+    this->set_parameters({
+        rclcpp::Parameter(name, alternative_value),
+      });
+    value = alternative_value;
+  }
+}
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__NODE_IMPL_HPP_

rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp

@@ -38,6 +38,10 @@ class NodeBaseInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeBaseInterface() = default;
+
   /// Return the name of the node.
   /** \return The name of the node. */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_clock.hpp

@@ -41,7 +41,8 @@ public:
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
     rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
     rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
-    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services);
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging);
 
   RCLCPP_PUBLIC
   virtual
@@ -60,9 +61,9 @@ private:
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
 
   rclcpp::Clock::SharedPtr ros_clock_;
-  rclcpp::TimeSource time_source_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp

@@ -31,6 +31,10 @@ class NodeClockInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeClockInterface() = default;
+
   /// Get a ROS clock which will be kept up to date by the node.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_graph.hpp

@@ -20,6 +20,7 @@
 #include <memory>
 #include <mutex>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl/guard_condition.h"
@@ -69,6 +70,11 @@ public:
   std::vector<std::string>
   get_node_names() const;
 
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::pair<std::string, std::string>>
+  get_node_names_and_namespaces() const;
+
   RCLCPP_PUBLIC
   virtual
   size_t

rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp

@@ -18,6 +18,7 @@
 #include <chrono>
 #include <map>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl/guard_condition.h"
@@ -37,6 +38,10 @@ class NodeGraphInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeGraphInterface() = default;
+
   /// 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
@@ -66,6 +71,12 @@ public:
   std::vector<std::string>
   get_node_names() const = 0;
 
+  /// Return a vector of existing node names and namespaces (pair of string).
+  RCLCPP_PUBLIC
+  virtual
+  std::vector<std::pair<std::string, std::string>>
+  get_node_names_and_namespaces() const = 0;
+
   /// Return the number of publishers that are advertised on a given topic.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp

@@ -33,6 +33,10 @@ class NodeLoggingInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeLoggingInterface() = default;
+
   /// Return the logger of the node.
   /** \return The logger of the node. */
   RCLCPP_PUBLIC

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -51,6 +51,7 @@ public:
     const node_interfaces::NodeBaseInterface::SharedPtr node_base,
     const node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
     const node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    const node_interfaces::NodeClockInterface::SharedPtr node_clock,
     const std::vector<Parameter> & initial_parameters,
     bool use_intra_process,
     bool start_parameter_services);
@@ -120,6 +121,10 @@ private:
   Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
 
   std::shared_ptr<ParameterService> parameter_service_;
+
+  std::string combined_name_;
+
+  node_interfaces::NodeClockInterface::SharedPtr node_clock_;
 };
 
 }  // namespace node_interfaces

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -37,6 +37,10 @@ class NodeParametersInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeParametersInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   std::vector<rcl_interfaces::msg::SetParametersResult>

rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp

@@ -32,6 +32,10 @@ class NodeServicesInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeServicesInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   void

rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp

@@ -0,0 +1,72 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_time_source_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/time_source.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeTimeSource part of the Node API.
+class NodeTimeSource : public NodeTimeSourceInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSource)
+
+  RCLCPP_PUBLIC
+  explicit NodeTimeSource(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters
+  );
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimeSource();
+
+private:
+  RCLCPP_DISABLE_COPY(NodeTimeSource)
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+
+  rclcpp::TimeSource time_source_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp

@@ -0,0 +1,42 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeTimeSource part of the Node API.
+class NodeTimeSourceInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSourceInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimeSourceInterface() = default;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_

rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp

@@ -31,6 +31,10 @@ class NodeTimersInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTimersInterface() = default;
+
   /// Add a timer to the node.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp

@@ -40,6 +40,10 @@ class NodeTopicsInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface)
 
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeTopicsInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   rclcpp::PublisherBase::SharedPtr

rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp

@@ -0,0 +1,67 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_waitables_interface.hpp"
+#include "rclcpp/waitable.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Implementation of the NodeWaitables part of the Node API.
+class NodeWaitables : public NodeWaitablesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitables)
+
+  RCLCPP_PUBLIC
+  explicit NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base);
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeWaitables();
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_waitable(
+    rclcpp::Waitable::SharedPtr waitable_base_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group);
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept;
+
+private:
+  RCLCPP_DISABLE_COPY(NodeWaitables)
+
+  rclcpp::node_interfaces::NodeBaseInterface * node_base_;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_

rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp

@@ -0,0 +1,57 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
+#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_
+
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
+
+namespace rclcpp
+{
+namespace node_interfaces
+{
+
+/// Pure virtual interface class for the NodeWaitables part of the Node API.
+class NodeWaitablesInterface
+{
+public:
+  RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface)
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeWaitablesInterface() = default;
+
+  RCLCPP_PUBLIC
+  virtual
+  void
+  add_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0;
+
+  /// \note this function should not throw because it may be called in destructors
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_waitable(
+    rclcpp::Waitable::SharedPtr waitable_ptr,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept = 0;
+};
+
+}  // namespace node_interfaces
+}  // namespace rclcpp
+
+#endif  // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_

rclcpp/include/rclcpp/publisher.hpp

@@ -205,6 +205,16 @@ public:
       ipm.publisher_id = intra_process_publisher_id_;
       ipm.message_sequence = message_seq;
       auto status = rcl_publish(&intra_process_publisher_handle_, &ipm);
+      if (RCL_RET_PUBLISHER_INVALID == status) {
+        rcl_reset_error();  // next call will reset error message if not context
+        if (rcl_publisher_is_valid_except_context(&intra_process_publisher_handle_)) {
+          rcl_context_t * context = rcl_publisher_get_context(&intra_process_publisher_handle_);
+          if (nullptr != context && !rcl_context_is_valid(context)) {
+            // publisher is invalid due to context being shutdown
+            return;
+          }
+        }
+      }
       if (RCL_RET_OK != status) {
         rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish intra process message");
       }
@@ -289,6 +299,12 @@ public:
     }
   }
 
+  void
+  publish(std::shared_ptr<const rcl_serialized_message_t> serialized_msg)
+  {
+    return this->publish(serialized_msg.get());
+  }
+
   std::shared_ptr<MessageAlloc> get_allocator() const
   {
     return message_allocator_;
@@ -299,6 +315,16 @@ protected:
   do_inter_process_publish(const MessageT * msg)
   {
     auto status = rcl_publish(&publisher_handle_, msg);
+    if (RCL_RET_PUBLISHER_INVALID == status) {
+      rcl_reset_error();  // next call will reset error message if not context
+      if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
+        rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
+        if (nullptr != context && !rcl_context_is_valid(context)) {
+          // publisher is invalid due to context being shutdown
+          return;
+        }
+      }
+    }
     if (RCL_RET_OK != status) {
       rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message");
     }

rclcpp/include/rclcpp/rclcpp.hpp

@@ -151,5 +151,6 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/waitable.hpp"
 
 #endif  // RCLCPP__RCLCPP_HPP_

rclcpp/include/rclcpp/service.hpp

@@ -89,12 +89,12 @@ class Service : public ServiceBase
 {
 public:
   using CallbackType = std::function<
-    void(
+    void (
       const std::shared_ptr<typename ServiceT::Request>,
       std::shared_ptr<typename ServiceT::Response>)>;
 
   using CallbackWithHeaderType = std::function<
-    void(
+    void (
       const std::shared_ptr<rmw_request_id_t>,
       const std::shared_ptr<typename ServiceT::Request>,
       std::shared_ptr<typename ServiceT::Response>)>;
@@ -119,9 +119,9 @@ public:
         if (handle) {
           if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
             RCLCPP_ERROR(
-              rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
+              rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
               "Error in destruction of rcl service handle: %s",
-              rcl_get_error_string_safe());
+              rcl_get_error_string().str);
             rcl_reset_error();
           }
         } else {
@@ -164,7 +164,7 @@ public:
     any_callback_(any_callback)
   {
     // check if service handle was initialized
-    if (!rcl_service_is_valid(service_handle.get(), nullptr)) {
+    if (!rcl_service_is_valid(service_handle.get())) {
       // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       throw std::runtime_error(
         std::string("rcl_service_t in constructor argument must be initialized beforehand."));
@@ -182,7 +182,7 @@ public:
     any_callback_(any_callback)
   {
     // check if service handle was initialized
-    if (!rcl_service_is_valid(service_handle, nullptr)) {
+    if (!rcl_service_is_valid(service_handle)) {
       // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
       throw std::runtime_error(
         std::string("rcl_service_t in constructor argument must be initialized beforehand."));

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -87,30 +87,42 @@ public:
     service_handles_.clear();
     client_handles_.clear();
     timer_handles_.clear();
+    waitable_handles_.clear();
   }
 
   virtual void remove_null_handles(rcl_wait_set_t * wait_set)
   {
-    for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) {
+    // TODO(jacobperron): Check if wait set sizes are what we expect them to be?
+    //                    e.g. wait_set->size_of_clients == client_handles_.size()
+
+    // Important to use subscription_handles_.size() instead of wait set's size since
+    // there may be more subscriptions in the wait set due to Waitables added to the end.
+    // The same logic applies for other entities.
+    for (size_t i = 0; i < subscription_handles_.size(); ++i) {
       if (!wait_set->subscriptions[i]) {
         subscription_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_services; ++i) {
+    for (size_t i = 0; i < service_handles_.size(); ++i) {
       if (!wait_set->services[i]) {
         service_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_clients; ++i) {
+    for (size_t i = 0; i < client_handles_.size(); ++i) {
       if (!wait_set->clients[i]) {
         client_handles_[i].reset();
       }
     }
-    for (size_t i = 0; i < wait_set->size_of_timers; ++i) {
+    for (size_t i = 0; i < timer_handles_.size(); ++i) {
       if (!wait_set->timers[i]) {
         timer_handles_[i].reset();
       }
     }
+    for (size_t i = 0; i < waitable_handles_.size(); ++i) {
+      if (!waitable_handles_[i]->is_ready(wait_set)) {
+        waitable_handles_[i].reset();
+      }
+    }
 
     subscription_handles_.erase(
       std::remove(subscription_handles_.begin(), subscription_handles_.end(), nullptr),
@@ -131,6 +143,11 @@ public:
       std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr),
       timer_handles_.end()
     );
+
+    waitable_handles_.erase(
+      std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr),
+      waitable_handles_.end()
+    );
   }
 
   bool collect_entities(const WeakNodeVector & weak_nodes)
@@ -175,6 +192,12 @@ public:
             timer_handles_.push_back(timer->get_timer_handle());
           }
         }
+        for (auto & weak_waitable : group->get_waitable_ptrs()) {
+          auto waitable = weak_waitable.lock();
+          if (waitable) {
+            waitable_handles_.push_back(waitable);
+          }
+        }
       }
     }
     return has_invalid_weak_nodes;
@@ -183,47 +206,56 @@ public:
   bool add_handles_to_wait_set(rcl_wait_set_t * wait_set)
   {
     for (auto subscription : subscription_handles_) {
-      if (rcl_wait_set_add_subscription(wait_set, subscription.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "Couldn't add subscription to wait set: %s", rcl_get_error_string_safe());
+          "Couldn't add subscription to wait set: %s", rcl_get_error_string().str);
         return false;
       }
     }
 
     for (auto client : client_handles_) {
-      if (rcl_wait_set_add_client(wait_set, client.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "Couldn't add client to wait set: %s", rcl_get_error_string_safe());
+          "Couldn't add client to wait set: %s", rcl_get_error_string().str);
         return false;
       }
     }
 
     for (auto service : service_handles_) {
-      if (rcl_wait_set_add_service(wait_set, service.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "Couldn't add service to wait set: %s", rcl_get_error_string_safe());
+          "Couldn't add service to wait set: %s", rcl_get_error_string().str);
         return false;
       }
     }
 
     for (auto timer : timer_handles_) {
-      if (rcl_wait_set_add_timer(wait_set, timer.get()) != RCL_RET_OK) {
+      if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "Couldn't add timer to wait set: %s", rcl_get_error_string_safe());
+          "Couldn't add timer to wait set: %s", rcl_get_error_string().str);
         return false;
       }
     }
 
     for (auto guard_condition : guard_conditions_) {
-      if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) {
+      if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
           "Couldn't add guard_condition to wait set: %s",
-          rcl_get_error_string_safe());
+          rcl_get_error_string().str);
+        return false;
+      }
+    }
+
+    for (auto waitable : waitable_handles_) {
+      if (!waitable->add_to_wait_set(wait_set)) {
+        RCUTILS_LOG_ERROR_NAMED(
+          "rclcpp",
+          "Couldn't add waitable to wait set: %s", rcl_get_error_string().str);
         return false;
       }
     }
@@ -342,6 +374,39 @@ public:
     }
   }
 
+  virtual void
+  get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes)
+  {
+    auto it = waitable_handles_.begin();
+    while (it != waitable_handles_.end()) {
+      auto waitable = *it;
+      if (waitable) {
+        // Find the group for this handle and see if it can be serviced
+        auto group = get_group_by_waitable(waitable, weak_nodes);
+        if (!group) {
+          // Group was not found, meaning the waitable is not valid...
+          // Remove it from the ready list and continue looking
+          it = waitable_handles_.erase(it);
+          continue;
+        }
+        if (!group->can_be_taken_from().load()) {
+          // Group is mutually exclusive and is being used, so skip it for now
+          // Leave it to be checked next time, but continue searching
+          ++it;
+          continue;
+        }
+        // Otherwise it is safe to set and return the any_exec
+        any_exec.waitable = waitable;
+        any_exec.callback_group = group;
+        any_exec.node_base = get_node_by_group(group, weak_nodes);
+        waitable_handles_.erase(it);
+        return;
+      }
+      // Else, the waitable is no longer valid, remove it and continue
+      it = waitable_handles_.erase(it);
+    }
+  }
+
   virtual rcl_allocator_t get_allocator()
   {
     return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
@@ -349,27 +414,52 @@ public:
 
   size_t number_of_ready_subscriptions() const
   {
-    return subscription_handles_.size();
+    size_t number_of_subscriptions = subscription_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_subscriptions += waitable->get_number_of_ready_subscriptions();
+    }
+    return number_of_subscriptions;
   }
 
   size_t number_of_ready_services() const
   {
-    return service_handles_.size();
+    size_t number_of_services = service_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_services += waitable->get_number_of_ready_services();
+    }
+    return number_of_services;
   }
 
   size_t number_of_ready_clients() const
   {
-    return client_handles_.size();
+    size_t number_of_clients = client_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_clients += waitable->get_number_of_ready_clients();
+    }
+    return number_of_clients;
   }
 
   size_t number_of_guard_conditions() const
   {
-    return guard_conditions_.size();
+    size_t number_of_guard_conditions = guard_conditions_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions();
+    }
+    return number_of_guard_conditions;
   }
 
   size_t number_of_ready_timers() const
   {
-    return timer_handles_.size();
+    size_t number_of_timers = timer_handles_.size();
+    for (auto waitable : waitable_handles_) {
+      number_of_timers += waitable->get_number_of_ready_timers();
+    }
+    return number_of_timers;
+  }
+
+  size_t number_of_waitables() const
+  {
+    return waitable_handles_.size();
   }
 
 private:
@@ -383,6 +473,7 @@ private:
   VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;
   VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
   VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
+  VectorRebind<std::shared_ptr<Waitable>> waitable_handles_;
 
   std::shared_ptr<VoidAlloc> allocator_;
 };

rclcpp/include/rclcpp/subscription.hpp

@@ -59,6 +59,7 @@ public:
    * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
    * \param[in] topic_name Name of the topic to subscribe to.
    * \param[in] subscription_options options for the subscription.
+   * \param[in] is_serialized is true if the message will be delivered still serialized
    */
   RCLCPP_PUBLIC
   SubscriptionBase(
@@ -161,6 +162,7 @@ public:
    * The constructor for a subscription is almost never called directly. Instead, subscriptions
    * should be instantiated through Node::create_subscription.
    * \param[in] node_handle rcl representation of the node that owns this subscription.
+   * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
    * \param[in] topic_name Name of the topic to subscribe to.
    * \param[in] subscription_options options for the subscription.
    * \param[in] callback User defined callback to call when a message is received.
@@ -168,7 +170,7 @@ public:
    */
   Subscription(
     std::shared_ptr<rcl_node_t> node_handle,
-    const rosidl_message_type_support_t & ts,
+    const rosidl_message_type_support_t & type_support_handle,
     const std::string & topic_name,
     const rcl_subscription_options_t & subscription_options,
     AnySubscriptionCallback<CallbackMessageT, Alloc> callback,
@@ -177,7 +179,7 @@ public:
     Alloc>::create_default())
   : SubscriptionBase(
       node_handle,
-      ts,
+      type_support_handle,
       topic_name,
       subscription_options,
       rclcpp::subscription_traits::is_serialized_subscription_argument<CallbackMessageT>::value),
@@ -266,8 +268,8 @@ public:
   }
 
   using GetMessageCallbackType =
-    std::function<void(uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
-  using MatchesAnyPublishersCallbackType = std::function<bool(const rmw_gid_t *)>;
+    std::function<void (uint64_t, uint64_t, uint64_t, MessageUniquePtr &)>;
+  using MatchesAnyPublishersCallbackType = std::function<bool (const rmw_gid_t *)>;
 
   /// Implemenation detail.
   void setup_intra_process(

rclcpp/include/rclcpp/subscription_factory.hpp

@@ -57,7 +57,7 @@ struct SubscriptionFactory
 
   // Function that takes a MessageT from the intra process manager
   using SetupIntraProcessFunction = std::function<
-    void(
+    void (
       rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm,
       rclcpp::SubscriptionBase::SharedPtr subscription,
       const rcl_subscription_options_t & subscription_options)>;

rclcpp/include/rclcpp/time.hpp

@@ -102,6 +102,17 @@ public:
   rcl_time_point_value_t
   nanoseconds() const;
 
+  RCLCPP_PUBLIC
+  static Time
+  max();
+
+  /// \return the seconds since epoch as a floating point number.
+  /// \warning Depending on sizeof(double) there could be significant precision loss.
+  /// When an exact time is required use nanoseconds() instead.
+  RCLCPP_PUBLIC
+  double
+  seconds() const;
+
   RCLCPP_PUBLIC
   rcl_clock_type_t
   get_clock_type() const;

rclcpp/include/rclcpp/time_source.hpp

@@ -47,10 +47,13 @@ public:
 
   RCLCPP_PUBLIC
   void attachNode(
-    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);
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface);
 
   RCLCPP_PUBLIC
   void detachNode();
@@ -74,16 +77,29 @@ private:
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+
+  // Store (and update on node attach) logger for logging.
+  Logger logger_;
 
   // The subscription for the clock callback
   using MessageT = rosgraph_msgs::msg::Clock;
   using Alloc = std::allocator<void>;
   using SubscriptionT = rclcpp::Subscription<MessageT, Alloc>;
   std::shared_ptr<SubscriptionT> clock_subscription_;
+  std::mutex clock_sub_lock_;
 
   // The clock callback itself
   void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg);
 
+  // Create the subscription for the clock topic
+  void create_clock_sub();
+
+  // Destroy the subscription for the clock topic
+  void destroy_clock_sub();
+
   // Parameter Client pointer
   std::shared_ptr<rclcpp::AsyncParametersClient> parameter_client_;
 

rclcpp/include/rclcpp/timer.hpp

@@ -23,6 +23,8 @@
 #include <type_traits>
 #include <utility>
 
+#include "rclcpp/clock.hpp"
+#include "rclcpp/context.hpp"
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/rate.hpp"
@@ -44,7 +46,10 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase)
 
   RCLCPP_PUBLIC
-  explicit TimerBase(std::chrono::nanoseconds period);
+  explicit TimerBase(
+    Clock::SharedPtr clock,
+    std::chrono::nanoseconds period,
+    rclcpp::Context::SharedPtr context);
 
   RCLCPP_PUBLIC
   ~TimerBase();
@@ -85,21 +90,20 @@ public:
   bool is_ready();
 
 protected:
+  Clock::SharedPtr clock_;
   std::shared_ptr<rcl_timer_t> timer_handle_;
 };
 
 
-using VoidCallbackType = std::function<void()>;
-using TimerCallbackType = std::function<void(TimerBase &)>;
+using VoidCallbackType = std::function<void ()>;
+using TimerCallbackType = std::function<void (TimerBase &)>;
 
-/// Generic timer templated on the clock type. Periodically executes a user-specified callback.
+/// Generic timer. Periodically executes a user-specified callback.
 template<
   typename FunctorT,
-  class Clock,
   typename std::enable_if<
-    (rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
-    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value) &&
-    Clock::is_steady
+    rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
   >::type * = nullptr
 >
 class GenericTimer : public TimerBase
@@ -109,11 +113,15 @@ public:
 
   /// Default constructor.
   /**
+   * \param[in] clock The clock providing the current time.
    * \param[in] period The interval at which the timer fires.
    * \param[in] callback User-specified callback function.
    */
-  GenericTimer(std::chrono::nanoseconds period, FunctorT && callback)
-  : TimerBase(period), callback_(std::forward<FunctorT>(callback))
+  explicit GenericTimer(
+    Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback,
+    rclcpp::Context::SharedPtr context
+  )
+  : TimerBase(clock, period, context), callback_(std::forward<FunctorT>(callback))
   {
   }
 
@@ -125,7 +133,7 @@ public:
   }
 
   void
-  execute_callback()
+  execute_callback() override
   {
     rcl_ret_t ret = rcl_timer_call(timer_handle_.get());
     if (ret == RCL_RET_TIMER_CANCELED) {
@@ -162,10 +170,10 @@ public:
     callback_(*this);
   }
 
-  virtual bool
-  is_steady()
+  bool
+  is_steady() override
   {
-    return Clock::is_steady;
+    return clock_->get_clock_type() == RCL_STEADY_TIME;
   }
 
 protected:
@@ -174,8 +182,29 @@ protected:
   FunctorT callback_;
 };
 
-template<typename CallbackType>
-using WallTimer = GenericTimer<CallbackType, std::chrono::steady_clock>;
+template<
+  typename FunctorT,
+  typename std::enable_if<
+    rclcpp::function_traits::same_arguments<FunctorT, VoidCallbackType>::value ||
+    rclcpp::function_traits::same_arguments<FunctorT, TimerCallbackType>::value
+  >::type * = nullptr
+>
+class WallTimer : public GenericTimer<FunctorT>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(WallTimer)
+
+  WallTimer(
+    std::chrono::nanoseconds period,
+    FunctorT && callback,
+    rclcpp::Context::SharedPtr context)
+  : GenericTimer<FunctorT>(
+      std::make_shared<Clock>(RCL_STEADY_TIME), period, std::move(callback), context)
+  {}
+
+protected:
+  RCLCPP_DISABLE_COPY(WallTimer)
+};
 
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/utilities.hpp

@@ -20,11 +20,10 @@
 #include <limits>
 #include <vector>
 
+#include "rclcpp/context.hpp"
+#include "rclcpp/init_options.hpp"
 #include "rclcpp/visibility_control.hpp"
 
-#include "rcl/guard_condition.h"
-#include "rcl/wait.h"
-
 #include "rmw/macros.h"
 #include "rmw/rmw.h"
 
@@ -48,28 +47,75 @@ namespace rclcpp
 {
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
- * \param[in] argc Number of arguments.
- * \param[in] argv Argument vector. Will eventually be used for passing options to rclcpp.
+ * Initializes the global context which is accessible via the function
+ * rclcpp::contexts::default_context::get_global_default_context().
+ * Also, installs the global signal handlers with the function
+ * rclcpp::install_signal_handlers().
+ *
+ * \sa rclcpp::Context::init() for more details on arguments and possible exceptions
  */
 RCLCPP_PUBLIC
 void
-init(int argc, char const * const argv[]);
+init(int argc, char const * const argv[], const InitOptions & init_options = InitOptions());
+
+/// Install the global signal handler for rclcpp.
+/**
+ * This function should only need to be run one time per process.
+ * It is implicitly run by rclcpp::init(), and therefore this function does not
+ * need to be run manually if rclcpp::init() has already been run.
+ *
+ * The signal handler will shutdown all initialized context.
+ * It will also interrupt any blocking functions in ROS allowing them react to
+ * any changes in the state of the system (like shutdown).
+ *
+ * This function is thread-safe.
+ *
+ * \return true if signal handler was installed by this function, false if already installed.
+ */
+RCLCPP_PUBLIC
+bool
+install_signal_handlers();
+
+/// Return true if the signal handlers are installed, otherwise false.
+RCLCPP_PUBLIC
+bool
+signal_handlers_installed();
+
+/// Uninstall the global signal handler for rclcpp.
+/**
+ * This function does not necessarily need to be called, but can be used to
+ * undo what rclcpp::install_signal_handlers() or rclcpp::init() do with
+ * respect to signal handling.
+ * If you choose to use it, this function only needs to be run one time.
+ * It is implicitly run by rclcpp::shutdown(), and therefore this function does
+ * not need to be run manually if rclcpp::shutdown() has already been run.
+ *
+ * This function is thread-safe.
+ *
+ * \return true if signal handler was uninstalled by this function, false if was not installed.
+ */
+RCLCPP_PUBLIC
+bool
+uninstall_signal_handlers();
 
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
  * Additionally removes ROS-specific arguments from the argument vector.
- * \param[in] argc Number of arguments.
- * \param[in] argv Argument vector.
+ *
+ * \sa rclcpp::Context::init() for more details on arguments and possible exceptions
  * \returns Members of the argument vector that are not ROS arguments.
  */
 RCLCPP_PUBLIC
 std::vector<std::string>
-init_and_remove_ros_arguments(int argc, char const * const argv[]);
+init_and_remove_ros_arguments(
+  int argc,
+  char const * const argv[],
+  const InitOptions & init_options = InitOptions());
 
 /// Remove ROS-specific arguments from argument vector.
 /**
  * Some arguments may not have been intended as ROS arguments.
- * This function populates a the aruments in a vector.
+ * This function populates the arguments in a vector.
  * Since the first argument is always assumed to be a process name, the vector
  * will always contain the process name.
  *
@@ -82,59 +128,91 @@ std::vector<std::string>
 remove_ros_arguments(int argc, char const * const argv[]);
 
 /// Check rclcpp's status.
-/** \return True if SIGINT hasn't fired yet, false otherwise. */
+/**
+ * This may return false for a context which has been shutdown, or for a
+ * context that was shutdown due to SIGINT being received by the rclcpp signal
+ * handler.
+ *
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * \param[in] context Check for shutdown of this Context.
+ * \return true if shutdown has been called, false otherwise
+ */
 RCLCPP_PUBLIC
 bool
-ok();
+ok(rclcpp::Context::SharedPtr context = nullptr);
 
-/// Notify the signal handler and rmw that rclcpp is shutting down.
-RCLCPP_PUBLIC
-void
-shutdown();
-
-/// Register a function to be called when shutdown is called.
-/** Calling the callbacks is the last thing shutdown() does. */
-RCLCPP_PUBLIC
-void
-on_shutdown(std::function<void(void)> callback);
-
-/// Get a handle to the rmw guard condition that manages the signal handler.
+/// Return true if init() has already been called for the given context.
 /**
- * The first time that this function is called for a given wait set a new guard
- * condition will be created and returned; thereafter the same guard condition
- * will be returned for the same wait set. This mechanism is designed to ensure
- * that the same guard condition is not reused across wait sets (e.g., when
- * using multiple executors in the same process). Will throw an exception if
- * initialization of the guard condition fails.
- * \param wait_set Pointer to the rcl_wait_set_t that will be using the
- * resulting guard condition.
- * \return Pointer to the guard condition.
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * Deprecated, as it is no longer different from rcl_ok().
+ *
+ * \param[in] context Check for initialization of this Context.
+ * \return true if the context is initialized, and false otherwise
  */
 RCLCPP_PUBLIC
-rcl_guard_condition_t *
-get_sigint_guard_condition(rcl_wait_set_t * wait_set);
+bool
+is_initialized(rclcpp::Context::SharedPtr context = nullptr);
 
-/// Release the previously allocated guard condition that manages the signal handler.
+/// Shutdown rclcpp context, invalidating it for derived entities.
 /**
- * If you previously called get_sigint_guard_condition() for a given wait set
- * 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 wait set.
- * \param wait_set Pointer to the rcl_wait_set_t that was using the
- * resulting guard condition.
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * If the global context is used, then the signal handlers are also uninstalled.
+ *
+ * This will also cause the "on_shutdown" callbacks to be called.
+ *
+ * \sa rclcpp::Context::shutdown()
+ * \param[in] context to be shutdown
+ * \return true if shutdown was successful, false if context was already shutdown
+ */
+RCLCPP_PUBLIC
+bool
+shutdown(
+  rclcpp::Context::SharedPtr context = nullptr,
+  const std::string & reason = "user called rclcpp::shutdown()");
+
+/// Register a function to be called when shutdown is called on the context.
+/**
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
+ * These callbacks are called when the associated Context is shutdown with the
+ * Context::shutdown() method.
+ * When shutdown by the SIGINT handler, shutdown, and therefore these callbacks,
+ * is called asynchronously from the dedicated signal handling thread, at some
+ * point after the SIGINT signal is received.
+ *
+ * \sa rclcpp::Context::on_shutdown()
+ * \param[in] callback to be called when the given context is shutdown
+ * \param[in] context with which to associate the context
  */
 RCLCPP_PUBLIC
 void
-release_sigint_guard_condition(rcl_wait_set_t * wait_set);
+on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr context = nullptr);
 
 /// Use the global condition variable to block for the specified amount of time.
 /**
+ * This function can be interrupted early if the associated context becomes
+ * invalid due to shutdown() or the signal handler.
+ * \sa rclcpp::Context::sleep_for
+ *
+ * If nullptr is given for the context, then the global context is used, i.e.
+ * the context initialized by rclcpp::init().
+ *
  * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for.
- * \return True if the condition variable did not timeout.
+ * \param[in] context which may interrupt this sleep
+ * \return true if the condition variable did not timeout.
  */
 RCLCPP_PUBLIC
 bool
-sleep_for(const std::chrono::nanoseconds & nanoseconds);
+sleep_for(
+  const std::chrono::nanoseconds & nanoseconds,
+  rclcpp::Context::SharedPtr context = nullptr);
 
 /// Safely check if addition will overflow.
 /**
@@ -204,6 +282,26 @@ sub_will_underflow(const T x, const T y)
   return (y > 0) && (x < (std::numeric_limits<T>::min() + y));
 }
 
+/// Return the given string.
+/**
+ * This function is overloaded to transform any string to C-style string.
+ *
+ * \param[in] string_in is the string to be returned
+ * \return the given string
+ */
+RCLCPP_PUBLIC
+const char *
+get_c_string(const char * string_in);
+
+/// Return the C string from the given std::string.
+/**
+ * \param[in] string_in is a std::string
+ * \return the C string from the std::string
+ */
+RCLCPP_PUBLIC
+const char *
+get_c_string(const std::string & string_in);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__UTILITIES_HPP_

rclcpp/include/rclcpp/waitable.hpp

@@ -0,0 +1,141 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__WAITABLE_HPP_
+#define RCLCPP__WAITABLE_HPP_
+
+#include "rclcpp/macros.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+#include "rcl/wait.h"
+
+namespace rclcpp
+{
+
+class Waitable
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Waitable)
+
+  /// Get the number of ready subscriptions
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more subscriptions.
+   * \return The number of subscriptions associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_subscriptions();
+
+  /// Get the number of ready timers
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more timers.
+   * \return The number of timers associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_timers();
+
+  /// Get the number of ready clients
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more clients.
+   * \return The number of clients associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_clients();
+
+  /// Get the number of ready services
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more services.
+   * \return The number of services associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_services();
+
+  /// Get the number of ready guard_conditions
+  /**
+   * Returns a value of 0 by default.
+   * This should be overridden if the Waitable contains one or more guard_conditions.
+   * \return The number of guard_conditions associated with the Waitable.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  size_t
+  get_number_of_ready_guard_conditions();
+
+  // TODO(jacobperron): smart pointer?
+  /// Add the Waitable to a wait set.
+  /**
+   * \param[in] wait_set A handle to the wait set to add the Waitable to.
+   * \return `true` if the Waitable is added successfully, `false` otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) = 0;
+
+  /// Check if the Waitable is ready.
+  /**
+   * The input wait set should be the same that was used in a previously call to
+   * `add_to_wait_set()`.
+   * The wait set should also have been previously waited on with `rcl_wait()`.
+   *
+   * \param[in] wait_set A handle to the wait set the Waitable was previously added to
+   *   and that has been waited on.
+   * \return `true` if the Waitable is ready, `false` otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  is_ready(rcl_wait_set_t *) = 0;
+
+  /// Execute any entities of the Waitable that are ready.
+  /**
+   * Before calling this method, the Waitable should be added to a wait set,
+   * waited on, and then updated.
+   *
+   * Example usage:
+   *
+   * ```
+   * // ... create a wait set and a Waitable
+   * // Add the Waitable to the wait set
+   * bool add_ret = waitable.add_to_wait_set(wait_set);
+   * // ... error handling
+   * // Wait
+   * rcl_ret_t wait_ret = rcl_wait(wait_set);
+   * // ... error handling
+   * // Update the Waitable
+   * waitable.update(wait_set);
+   * // Execute any entities of the Waitable that may be ready
+   * waitable.execute();
+   * ```
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  execute() = 0;
+};  // class Waitable
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__WAITABLE_HPP_

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

rclcpp/resource/logging.hpp.em

@@ -21,6 +21,7 @@
 
 #include "rclcpp/logger.hpp"
 #include "rcutils/logging_macros.h"
+#include "rclcpp/utilities.hpp"
 
 // These are used for compiling out logging macros lower than a minimum severity.
 #define RCLCPP_LOG_MIN_SEVERITY_DEBUG 0
@@ -30,6 +31,9 @@
 #define RCLCPP_LOG_MIN_SEVERITY_FATAL 4
 #define RCLCPP_LOG_MIN_SEVERITY_NONE 5
 
+#define RCLCPP_FIRST_ARG(N, ...) N
+#define RCLCPP_ALL_BUT_FIRST_ARGS(N, ...) __VA_ARGS__
+
 /**
  * \def RCLCPP_LOG_MIN_SEVERITY
  * Define RCLCPP_LOG_MIN_SEVERITY=RCLCPP_LOG_MIN_SEVERITY_[DEBUG|INFO|WARN|ERROR|FATAL]
@@ -82,11 +86,13 @@ def is_supported_feature_combination(feature_combination):
 @[ for param_name, doc_line in feature_combinations[feature_combination].params.items()]@
  * \param @(param_name) @(doc_line)
 @[ end for]@
- * \param ... The format string, followed by the variable arguments for the format string
+ * \param ... The format string, followed by the variable arguments for the format string.
+ * It also accepts a single argument of type std::string.
  */
 #define RCLCPP_@(severity)@(suffix)(logger, @(''.join([p + ', ' for p in get_macro_parameters(feature_combination).keys()]))...) \
   static_assert( \
-    ::std::is_same<std::remove_reference<decltype(logger)>::type, ::rclcpp::Logger>::value, \
+    ::std::is_same<typename std::remove_reference<decltype(logger)>::type, \
+    typename ::rclcpp::Logger>::value, \
     "First argument to logging macros must be an rclcpp::Logger"); \
   RCUTILS_LOG_@(severity)@(suffix)_NAMED( \
 @{params = get_macro_parameters(feature_combination).keys()}@
@@ -94,7 +100,8 @@ def is_supported_feature_combination(feature_combination):
 @(''.join(['    ' + p + ', \\\n' for p in params]))@
 @[ end if]@
     logger.get_name(), \
-    __VA_ARGS__)
+    rclcpp::get_c_string(RCLCPP_FIRST_ARG(__VA_ARGS__, "")), \
+      RCLCPP_ALL_BUT_FIRST_ARGS(__VA_ARGS__,""))
 
 @[ end for]@
 #endif

rclcpp/src/rclcpp/callback_group.cpp

@@ -51,6 +51,13 @@ CallbackGroup::get_client_ptrs() const
   return client_ptrs_;
 }
 
+const std::vector<rclcpp::Waitable::WeakPtr> &
+CallbackGroup::get_waitable_ptrs() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return waitable_ptrs_;
+}
+
 std::atomic_bool &
 CallbackGroup::can_be_taken_from()
 {
@@ -91,3 +98,23 @@ CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr)
   std::lock_guard<std::mutex> lock(mutex_);
   client_ptrs_.push_back(client_ptr);
 }
+
+void
+CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  waitable_ptrs_.push_back(waitable_ptr);
+}
+
+void
+CallbackGroup::remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  for (auto iter = waitable_ptrs_.begin(); iter != waitable_ptrs_.end(); ++iter) {
+    const auto shared_ptr = iter->lock();
+    if (shared_ptr.get() == waitable_ptr.get()) {
+      waitable_ptrs_.erase(iter);
+      break;
+    }
+  }
+}

rclcpp/src/rclcpp/client.cpp

@@ -37,18 +37,21 @@ ClientBase::ClientBase(
   rclcpp::node_interfaces::NodeBaseInterface * node_base,
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph)
 : node_graph_(node_graph),
-  node_handle_(node_base->get_shared_rcl_node_handle())
+  node_handle_(node_base->get_shared_rcl_node_handle()),
+  context_(node_base->get_context())
 {
   std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
-  client_handle_ = std::shared_ptr<rcl_client_t>(
-    new rcl_client_t, [weak_node_handle](rcl_client_t * client)
+  rcl_client_t * new_rcl_client = new rcl_client_t;
+  *new_rcl_client = rcl_get_zero_initialized_client();
+  client_handle_.reset(
+    new_rcl_client, [weak_node_handle](rcl_client_t * client)
     {
       auto handle = weak_node_handle.lock();
       if (handle) {
         if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) {
           RCLCPP_ERROR(
-            rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
-            "Error in destruction of rcl client handle: %s", rcl_get_error_string_safe());
+            rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
+            "Error in destruction of rcl client handle: %s", rcl_get_error_string().str);
           rcl_reset_error();
         }
       } else {
@@ -59,7 +62,6 @@ ClientBase::ClientBase(
       }
       delete client;
     });
-  *client_handle_.get() = rcl_get_zero_initialized_client();
 }
 
 ClientBase::~ClientBase()
@@ -94,6 +96,13 @@ ClientBase::service_is_ready() const
     this->get_rcl_node_handle(),
     this->get_client_handle().get(),
     &is_ready);
+  if (RCL_RET_NODE_INVALID == ret) {
+    const rcl_node_t * node_handle = this->get_rcl_node_handle();
+    if (node_handle && !rcl_context_is_valid(node_handle->context)) {
+      // context is shutdown, do a soft failure
+      return false;
+    }
+  }
   if (ret != RCL_RET_OK) {
     throw_from_rcl_error(ret, "rcl_service_server_is_available failed");
   }
@@ -120,15 +129,17 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
   }
   // 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);
-  if (timeout > std::chrono::nanoseconds(0) && time_to_wait < std::chrono::nanoseconds(0)) {
+  std::chrono::nanoseconds time_to_wait =
+    timeout > std::chrono::nanoseconds(0) ?
+    timeout - (std::chrono::steady_clock::now() - start) :
+    std::chrono::nanoseconds::max();
+  if (time_to_wait < std::chrono::nanoseconds(0)) {
     // Do not allow the time_to_wait to become negative when timeout was originally positive.
     // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while.
     time_to_wait = std::chrono::nanoseconds(0);
   }
-  // continue forever if timeout is negative, otherwise continue until out of time_to_wait
   do {
-    if (!rclcpp::ok()) {
+    if (!rclcpp::ok(this->context_)) {
       return false;
     }
     // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation.
@@ -147,8 +158,11 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
       return true;
     }
     // server is not ready, loop if there is time left
-    time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
-  } while (time_to_wait > std::chrono::nanoseconds(0) || timeout < std::chrono::nanoseconds(0));
+    if (timeout > std::chrono::nanoseconds(0)) {
+      time_to_wait = timeout - (std::chrono::steady_clock::now() - start);
+    }
+    // if timeout is negative, time_to_wait will never reach zero
+  } while (time_to_wait > std::chrono::nanoseconds(0));
   return false;  // timeout exceeded while waiting for the server to be ready
 }
 

rclcpp/src/rclcpp/clock.cpp

@@ -28,27 +28,11 @@
 
 namespace rclcpp
 {
-bool
-JumpThreshold::is_exceeded(const TimeJump & jump)
-{
-  if (on_clock_change_ &&
-    (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED ||
-    jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED))
-  {
-    return true;
-  }
-  if ((uint64_t)jump.delta_.nanoseconds > min_forward_ ||
-    (uint64_t)jump.delta_.nanoseconds < min_backward_)
-  {
-    return true;
-  }
-  return false;
-}
 
 JumpHandler::JumpHandler(
   std::function<void()> pre_callback,
-  std::function<void(TimeJump)> post_callback,
-  JumpThreshold & threshold)
+  std::function<void(const rcl_time_jump_t &)> post_callback,
+  const rcl_jump_threshold_t & threshold)
 : pre_callback(pre_callback),
   post_callback(post_callback),
   notice_threshold(threshold)
@@ -77,7 +61,7 @@ Clock::now()
 {
   Time now(0, 0, rcl_clock_.type);
 
-  auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_);
+  auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_.nanoseconds);
   if (ret != RCL_RET_OK) {
     rclcpp::exceptions::throw_from_rcl_error(
       ret, "could not get current time stamp");
@@ -103,68 +87,65 @@ Clock::ros_time_is_active()
   return is_enabled;
 }
 
+rcl_clock_t *
+Clock::get_clock_handle()
+{
+  return &rcl_clock_;
+}
+
 rcl_clock_type_t
 Clock::get_clock_type()
 {
   return rcl_clock_.type;
 }
 
+void
+Clock::on_time_jump(
+  const struct rcl_time_jump_t * time_jump,
+  bool before_jump,
+  void * user_data)
+{
+  rclcpp::JumpHandler * handler = static_cast<rclcpp::JumpHandler *>(user_data);
+  if (before_jump && handler->pre_callback) {
+    handler->pre_callback();
+  } else if (!before_jump && handler->post_callback) {
+    handler->post_callback(*time_jump);
+  }
+}
+
 rclcpp::JumpHandler::SharedPtr
 Clock::create_jump_callback(
   std::function<void()> pre_callback,
-  std::function<void(const TimeJump &)> post_callback,
-  JumpThreshold & threshold)
+  std::function<void(const rcl_time_jump_t &)> post_callback,
+  const rcl_jump_threshold_t & threshold)
 {
-  // JumpHandler jump_callback;
-  auto jump_callback =
-    std::make_shared<rclcpp::JumpHandler>(pre_callback, post_callback, threshold);
-  {
-    std::lock_guard<std::mutex> guard(callback_list_mutex_);
-    active_jump_handlers_.push_back(jump_callback);
+  // Allocate a new jump handler
+  auto handler = new rclcpp::JumpHandler(pre_callback, post_callback, threshold);
+  if (nullptr == handler) {
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_BAD_ALLOC, "Failed to allocate jump handler");
   }
-  return jump_callback;
-}
 
-std::vector<JumpHandler::SharedPtr>
-Clock::get_triggered_callback_handlers(const TimeJump & jump)
-{
-  std::vector<JumpHandler::SharedPtr> callbacks;
-  std::lock_guard<std::mutex> guard(callback_list_mutex_);
-  active_jump_handlers_.erase(
-    std::remove_if(
-      active_jump_handlers_.begin(),
-      active_jump_handlers_.end(),
-      [&callbacks, &jump](const std::weak_ptr<JumpHandler> & wjcb) {
-        if (auto jcb = wjcb.lock()) {
-          if (jcb->notice_threshold.is_exceeded(jump)) {
-            callbacks.push_back(jcb);
-          }
-          return false;
-        }
-        // Lock failed so clear the weak pointer.
-        return true;
-      }),
-    active_jump_handlers_.end());
-  return callbacks;
-}
-
-void
-Clock::invoke_prejump_callbacks(
-  const std::vector<JumpHandler::SharedPtr> & callbacks)
-{
-  for (const auto cb : callbacks) {
-    cb->pre_callback();
+  // Try to add the jump callback to the clock
+  rcl_ret_t ret = rcl_clock_add_jump_callback(&rcl_clock_, threshold,
+      rclcpp::Clock::on_time_jump, handler);
+  if (RCL_RET_OK != ret) {
+    delete handler;
+    handler = NULL;
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback");
   }
-}
 
-void
-Clock::invoke_postjump_callbacks(
-  const std::vector<JumpHandler::SharedPtr> & callbacks,
-  const TimeJump & jump)
-{
-  for (auto cb : callbacks) {
-    cb->post_callback(jump);
-  }
+  // *INDENT-OFF*
+  // create shared_ptr that removes the callback automatically when all copies are destructed
+  return rclcpp::JumpHandler::SharedPtr(handler, [this](rclcpp::JumpHandler * handler) noexcept {
+    rcl_ret_t ret = rcl_clock_remove_jump_callback(&rcl_clock_, rclcpp::Clock::on_time_jump,
+        handler);
+    delete handler;
+    handler = NULL;
+    if (RCL_RET_OK != ret) {
+      RCUTILS_LOG_ERROR("Failed to remove time jump callback");
+    }
+  });
+  // *INDENT-ON*
 }
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/context.cpp

@@ -14,6 +14,296 @@
 
 #include "rclcpp/context.hpp"
 
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "rcl/init.h"
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+#include "rmw/impl/cpp/demangle.hpp"
+
+/// Mutex to protect initialized contexts.
+static std::mutex g_contexts_mutex;
+/// Weak list of context to be shutdown by the signal handler.
+static std::vector<std::weak_ptr<rclcpp::Context>> g_contexts;
+
 using rclcpp::Context;
 
-Context::Context() {}
+Context::Context()
+: rcl_context_(nullptr), shutdown_reason_("") {}
+
+Context::~Context()
+{
+  // acquire the init lock to prevent race conditions with init and shutdown
+  // this will not prevent errors, but will maybe make them easier to reproduce
+  std::lock_guard<std::recursive_mutex> lock(init_mutex_);
+  try {
+    this->shutdown("context destructor was called while still not shutdown");
+    // at this point it is shutdown and cannot reinit
+    // clean_up will finalize the rcl context
+    this->clean_up();
+  } catch (const std::exception & exc) {
+    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context(): %s", exc.what());
+  } catch (...) {
+    RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context()");
+  }
+}
+
+RCLCPP_LOCAL
+void
+__delete_context(rcl_context_t * context)
+{
+  if (context) {
+    if (rcl_context_is_valid(context)) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"), "rcl context unexpectedly not shutdown during cleanup");
+    } else {
+      // if context pointer is not null and is shutdown, then it's ready for fini
+      rcl_ret_t ret = rcl_context_fini(context);
+      if (RCL_RET_OK != ret) {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"),
+          "failed to finalize context: %s", rcl_get_error_string().str);
+        rcl_reset_error();
+      }
+    }
+    delete context;
+  }
+}
+
+void
+Context::init(
+  int argc,
+  char const * const argv[],
+  const rclcpp::InitOptions & init_options)
+{
+  std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);
+  if (this->is_valid()) {
+    throw rclcpp::ContextAlreadyInitialized();
+  }
+  this->clean_up();
+  rcl_context_.reset(new rcl_context_t, __delete_context);
+  *rcl_context_.get() = rcl_get_zero_initialized_context();
+  rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), rcl_context_.get());
+  if (RCL_RET_OK != ret) {
+    rcl_context_.reset();
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
+  }
+  init_options_ = init_options;
+
+  std::lock_guard<std::mutex> lock(g_contexts_mutex);
+  g_contexts.push_back(this->shared_from_this());
+}
+
+bool
+Context::is_valid() const
+{
+  // Take a local copy of the shared pointer to avoid it getting nulled under our feet.
+  auto local_rcl_context = rcl_context_;
+  if (!local_rcl_context) {
+    return false;
+  }
+  return rcl_context_is_valid(local_rcl_context.get());
+}
+
+const rclcpp::InitOptions &
+Context::get_init_options() const
+{
+  return init_options_;
+}
+
+rclcpp::InitOptions
+Context::get_init_options()
+{
+  return init_options_;
+}
+
+std::string
+Context::shutdown_reason()
+{
+  std::lock_guard<std::recursive_mutex> lock(init_mutex_);
+  return shutdown_reason_;
+}
+
+bool
+Context::shutdown(const std::string & reason)
+{
+  // prevent races
+  std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);
+  // ensure validity
+  if (!this->is_valid()) {
+    // if it is not valid, then it cannot be shutdown
+    return false;
+  }
+  // rcl shutdown
+  rcl_ret_t ret = rcl_shutdown(rcl_context_.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+  // set shutdown reason
+  shutdown_reason_ = reason;
+  // call each shutdown callback
+  for (const auto & callback : on_shutdown_callbacks_) {
+    callback();
+  }
+  // interrupt all blocking sleep_for() and all blocking executors or wait sets
+  this->interrupt_all_sleep_for();
+  this->interrupt_all_wait_sets();
+  // remove self from the global contexts
+  std::lock_guard<std::mutex> context_lock(g_contexts_mutex);
+  for (auto it = g_contexts.begin(); it != g_contexts.end(); ) {
+    auto shared_context = it->lock();
+    if (shared_context.get() == this) {
+      it = g_contexts.erase(it);
+      break;
+    } else {
+      ++it;
+    }
+  }
+  return true;
+}
+
+rclcpp::Context::OnShutdownCallback
+Context::on_shutdown(OnShutdownCallback callback)
+{
+  on_shutdown_callbacks_.push_back(callback);
+  return callback;
+}
+
+const std::vector<rclcpp::Context::OnShutdownCallback> &
+Context::get_on_shutdown_callbacks() const
+{
+  return on_shutdown_callbacks_;
+}
+
+std::vector<rclcpp::Context::OnShutdownCallback> &
+Context::get_on_shutdown_callbacks()
+{
+  return on_shutdown_callbacks_;
+}
+
+std::shared_ptr<rcl_context_t>
+Context::get_rcl_context()
+{
+  return rcl_context_;
+}
+
+bool
+Context::sleep_for(const std::chrono::nanoseconds & nanoseconds)
+{
+  std::chrono::nanoseconds time_left = nanoseconds;
+  {
+    std::unique_lock<std::mutex> lock(interrupt_mutex_);
+    auto start = std::chrono::steady_clock::now();
+    // this will release the lock while waiting
+    interrupt_condition_variable_.wait_for(lock, nanoseconds);
+    time_left -= std::chrono::steady_clock::now() - start;
+  }
+  if (time_left > std::chrono::nanoseconds::zero() && this->is_valid()) {
+    return sleep_for(time_left);
+  }
+  // Return true if the timeout elapsed successfully, otherwise false.
+  return this->is_valid();
+}
+
+void
+Context::interrupt_all_sleep_for()
+{
+  interrupt_condition_variable_.notify_all();
+}
+
+rcl_guard_condition_t *
+Context::get_interrupt_guard_condition(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
+  auto kv = interrupt_guard_cond_handles_.find(wait_set);
+  if (kv != interrupt_guard_cond_handles_.end()) {
+    return &kv->second;
+  } else {
+    rcl_guard_condition_t handle = rcl_get_zero_initialized_guard_condition();
+    rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options();
+    auto ret = rcl_guard_condition_init(&handle, this->get_rcl_context().get(), options);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize guard condition");
+    }
+    interrupt_guard_cond_handles_.emplace(wait_set, handle);
+    return &interrupt_guard_cond_handles_[wait_set];
+  }
+}
+
+void
+Context::release_interrupt_guard_condition(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
+  auto kv = interrupt_guard_cond_handles_.find(wait_set);
+  if (kv != interrupt_guard_cond_handles_.end()) {
+    rcl_ret_t ret = rcl_guard_condition_fini(&kv->second);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to destroy sigint guard condition");
+    }
+    interrupt_guard_cond_handles_.erase(kv);
+  } else {
+    throw std::runtime_error("Tried to release sigint guard condition for nonexistent wait set");
+  }
+}
+
+void
+Context::release_interrupt_guard_condition(
+  rcl_wait_set_t * wait_set,
+  const std::nothrow_t &) noexcept
+{
+  try {
+    this->release_interrupt_guard_condition(wait_set);
+  } catch (const std::exception & exc) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "caught %s exception when releasing interrupt guard condition: %s",
+      rmw::impl::cpp::demangle(exc).c_str(), exc.what());
+  } catch (...) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "caught unknown exception when releasing interrupt guard condition");
+  }
+}
+
+void
+Context::interrupt_all_wait_sets()
+{
+  std::lock_guard<std::mutex> lock(interrupt_guard_cond_handles_mutex_);
+  for (auto & kv : interrupt_guard_cond_handles_) {
+    rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second));
+    if (status != RCL_RET_OK) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rclcpp",
+        "failed to trigger guard condition in Context::interrupt_all_wait_sets(): %s",
+        rcl_get_error_string().str);
+    }
+  }
+}
+
+void
+Context::clean_up()
+{
+  shutdown_reason_ = "";
+  rcl_context_.reset();
+}
+
+std::vector<Context::SharedPtr>
+rclcpp::get_contexts()
+{
+  std::lock_guard<std::mutex> lock(g_contexts_mutex);
+  std::vector<Context::SharedPtr> shared_contexts;
+  for (auto it = g_contexts.begin(); it != g_contexts.end(); /* noop */) {
+    auto context_ptr = it->lock();
+    if (!context_ptr) {
+      // remove invalid weak context pointers
+      it = g_contexts.erase(it);
+    } else {
+      ++it;
+      shared_contexts.push_back(context_ptr);
+    }
+  }
+  return shared_contexts;
+}

rclcpp/src/rclcpp/duration.cpp

@@ -88,9 +88,6 @@ Duration::operator=(const Duration & rhs)
 Duration &
 Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg)
 {
-  if (duration_msg.sec < 0) {
-    throw std::runtime_error("cannot store a negative duration point in rclcpp::Duration");
-  }
   rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(duration_msg.sec));
   rcl_duration_.nanoseconds += duration_msg.nanosec;
   return *this;
@@ -217,4 +214,16 @@ Duration::nanoseconds() const
   return rcl_duration_.nanoseconds;
 }
 
+Duration
+Duration::max()
+{
+  return Duration(std::numeric_limits<int32_t>::max(), 999999999);
+}
+
+double
+Duration::seconds() const
+{
+  return std::chrono::duration<double>(std::chrono::nanoseconds(rcl_duration_.nanoseconds)).count();
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/exceptions.cpp

@@ -44,7 +44,7 @@ throw_from_rcl_error(
   rcl_ret_t ret,
   const std::string & prefix,
   const rcl_error_state_t * error_state,
-  void (*reset_error)())
+  void (* reset_error)())
 {
   if (RCL_RET_OK == ret) {
     throw std::invalid_argument("ret is RCL_RET_OK");
@@ -75,7 +75,7 @@ throw_from_rcl_error(
 
 RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state)
 : ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number),
-  formatted_message(rcl_get_error_string_safe())
+  formatted_message(rcl_get_error_string().str)
 {}
 
 RCLError::RCLError(

rclcpp/src/rclcpp/executor.cpp

@@ -30,6 +30,7 @@
 
 #include "rcutils/logging_macros.h"
 
+using rclcpp::exceptions::throw_from_rcl_error;
 using rclcpp::executor::AnyExecutable;
 using rclcpp::executor::Executor;
 using rclcpp::executor::ExecutorArgs;
@@ -40,35 +41,36 @@ Executor::Executor(const ExecutorArgs & args)
   memory_strategy_(args.memory_strategy)
 {
   rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
-  if (rcl_guard_condition_init(
-      &interrupt_guard_condition_, guard_condition_options) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Failed to create interrupt guard condition in Executor constructor: ") +
-            rcl_get_error_string_safe());
+  rcl_ret_t ret = rcl_guard_condition_init(
+    &interrupt_guard_condition_, args.context->get_rcl_context().get(), guard_condition_options);
+  if (RCL_RET_OK != ret) {
+    throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor");
   }
 
   // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond,
   // and one for the executor's guard cond (interrupt_guard_condition_)
 
   // Put the global ctrl-c guard condition in
-  memory_strategy_->add_guard_condition(rclcpp::get_sigint_guard_condition(&wait_set_));
+  memory_strategy_->add_guard_condition(args.context->get_interrupt_guard_condition(&wait_set_));
 
   // Put the executor's guard condition in
   memory_strategy_->add_guard_condition(&interrupt_guard_condition_);
   rcl_allocator_t allocator = memory_strategy_->get_allocator();
 
+  // Store the context for later use.
+  context_ = args.context;
+
   if (rcl_wait_set_init(
       &wait_set_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK)
   {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
-      "failed to create wait set: %s", rcl_get_error_string_safe());
+      "failed to create wait set: %s", rcl_get_error_string().str);
     rcl_reset_error();
     if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
-        "failed to destroy guard condition: %s", rcl_get_error_string_safe());
+        "failed to destroy guard condition: %s", rcl_get_error_string().str);
       rcl_reset_error();
     }
     throw std::runtime_error("Failed to create wait set in Executor constructor");
@@ -91,20 +93,19 @@ Executor::~Executor()
   if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
-      "failed to destroy wait set: %s", rcl_get_error_string_safe());
+      "failed to destroy wait set: %s", rcl_get_error_string().str);
     rcl_reset_error();
   }
   // Finalize the interrupt guard condition.
   if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
-      "failed to destroy guard condition: %s", rcl_get_error_string_safe());
+      "failed to destroy guard condition: %s", rcl_get_error_string().str);
     rcl_reset_error();
   }
   // Remove and release the sigint guard condition
-  memory_strategy_->remove_guard_condition(
-    rclcpp::get_sigint_guard_condition(&wait_set_));
-  rclcpp::release_sigint_guard_condition(&wait_set_);
+  memory_strategy_->remove_guard_condition(context_->get_interrupt_guard_condition(&wait_set_));
+  context_->release_interrupt_guard_condition(&wait_set_, std::nothrow);
 }
 
 void
@@ -127,7 +128,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
   if (notify) {
     // Interrupt waiting to handle new node
     if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-      throw std::runtime_error(rcl_get_error_string_safe());
+      throw std::runtime_error(rcl_get_error_string().str);
     }
   }
   // Add the node's notify condition to the guard condition handles
@@ -161,7 +162,7 @@ Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node
     // If the node was matched and removed, interrupt waiting
     if (node_removed) {
       if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-        throw std::runtime_error(rcl_get_error_string_safe());
+        throw std::runtime_error(rcl_get_error_string().str);
       }
     }
   }
@@ -200,13 +201,26 @@ Executor::spin_node_some(std::shared_ptr<rclcpp::Node> node)
 }
 
 void
-Executor::spin_some()
+Executor::spin_some(std::chrono::nanoseconds max_duration)
 {
+  auto start = std::chrono::steady_clock::now();
+  auto max_duration_not_elapsed = [max_duration, start]() {
+      if (std::chrono::nanoseconds(0) == max_duration) {
+        // told to spin forever if need be
+        return true;
+      } else if (std::chrono::steady_clock::now() - start < max_duration) {
+        // told to spin only for some maximum amount of time
+        return true;
+      }
+      // spun too long
+      return false;
+    };
+
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_some() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  while (spinning.load()) {
+  while (spinning.load() && max_duration_not_elapsed()) {
     AnyExecutable any_exec;
     if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) {
       execute_any_executable(any_exec);
@@ -234,7 +248,7 @@ Executor::cancel()
 {
   spinning.store(false);
   if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string_safe());
+    throw std::runtime_error(rcl_get_error_string().str);
   }
 }
 
@@ -268,12 +282,15 @@ Executor::execute_any_executable(AnyExecutable & any_exec)
   if (any_exec.client) {
     execute_client(any_exec.client);
   }
+  if (any_exec.waitable) {
+    any_exec.waitable->execute();
+  }
   // Reset the callback_group, regardless of type
   any_exec.callback_group->can_be_taken_from().store(true);
   // Wake the wait, because it may need to be recalculated or work that
   // was previously blocked is now available.
   if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(rcl_get_error_string_safe());
+    throw std::runtime_error(rcl_get_error_string().str);
   }
 }
 
@@ -296,7 +313,7 @@ Executor::execute_subscription(
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
         "take_serialized failed for subscription on topic '%s': %s",
-        subscription->get_topic_name(), rcl_get_error_string_safe());
+        subscription->get_topic_name(), rcl_get_error_string().str);
       rcl_reset_error();
     }
     subscription->return_serialized_message(serialized_msg);
@@ -311,7 +328,7 @@ Executor::execute_subscription(
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
         "could not deserialize serialized message on topic '%s': %s",
-        subscription->get_topic_name(), rcl_get_error_string_safe());
+        subscription->get_topic_name(), rcl_get_error_string().str);
       rcl_reset_error();
     }
     subscription->return_message(message);
@@ -336,7 +353,7 @@ Executor::execute_intra_process_subscription(
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
       "take failed for intra process subscription on topic '%s': %s",
-      subscription->get_topic_name(), rcl_get_error_string_safe());
+      subscription->get_topic_name(), rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -364,7 +381,7 @@ Executor::execute_service(
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
       "take request failed for server of service '%s': %s",
-      service->get_service_name(), rcl_get_error_string_safe());
+      service->get_service_name(), rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -385,7 +402,7 @@ Executor::execute_client(
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
       "take response failed for client of service '%s': %s",
-      client->get_service_name(), rcl_get_error_string_safe());
+      client->get_service_name(), rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -410,60 +427,18 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     );
   }
   // clear wait set
-  if (rcl_wait_set_clear_subscriptions(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear subscriptions from wait set");
-  }
-  if (rcl_wait_set_clear_services(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear servicess from wait set");
-  }
-  if (rcl_wait_set_clear_clients(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear clients from wait set");
-  }
-  if (rcl_wait_set_clear_guard_conditions(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear guard conditions from wait set");
-  }
-  if (rcl_wait_set_clear_timers(&wait_set_) != RCL_RET_OK) {
-    throw std::runtime_error("Couldn't clear timers from wait set");
+  if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {
+    throw std::runtime_error("Couldn't clear wait set");
   }
 
-  if (rcl_wait_set_resize_subscriptions(
-      &wait_set_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK)
-  {
+  // The size of waitables are accounted for in size of the other entities
+  rcl_ret_t ret = rcl_wait_set_resize(
+    &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
+    memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
+    memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services());
+  if (RCL_RET_OK != ret) {
     throw std::runtime_error(
-            std::string("Couldn't resize the number of subscriptions in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_services(
-      &wait_set_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of services in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_clients(
-      &wait_set_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of clients in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_guard_conditions(
-      &wait_set_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of guard_conditions in wait set : ") +
-            rcl_get_error_string_safe());
-  }
-
-  if (rcl_wait_set_resize_timers(
-      &wait_set_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK)
-  {
-    throw std::runtime_error(
-            std::string("Couldn't resize the number of timers in wait set : ") +
-            rcl_get_error_string_safe());
+            std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str);
   }
 
   if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
@@ -579,6 +554,11 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
   if (any_executable.client) {
     return true;
   }
+  // Check the waitables to see if there are any that are ready
+  memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
+  if (any_executable.waitable) {
+    return true;
+  }
   // If there is no ready executable, return a null ptr
   return false;
 }
@@ -628,7 +608,7 @@ rclcpp::executor::operator<<(std::ostream & os, const FutureReturnCode & future_
 }
 
 std::string
-rclcpp::executor::to_string(const FutureReturnCode &future_return_code)
+rclcpp::executor::to_string(const FutureReturnCode & future_return_code)
 {
   using enum_type = std::underlying_type<FutureReturnCode>::type;
   std::string prefix = "Unknown enum value (";

rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp

@@ -70,11 +70,11 @@ MultiThreadedExecutor::get_number_of_threads()
 void
 MultiThreadedExecutor::run(size_t)
 {
-  while (rclcpp::ok() && spinning.load()) {
+  while (rclcpp::ok(this->context_) && spinning.load()) {
     executor::AnyExecutable any_exec;
     {
       std::lock_guard<std::mutex> wait_lock(wait_mutex_);
-      if (!rclcpp::ok() || !spinning.load()) {
+      if (!rclcpp::ok(this->context_) || !spinning.load()) {
         return;
       }
       if (!get_next_executable(any_exec)) {

rclcpp/src/rclcpp/executors/single_threaded_executor.cpp

@@ -30,7 +30,7 @@ SingleThreadedExecutor::spin()
     throw std::runtime_error("spin() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  while (rclcpp::ok() && spinning.load()) {
+  while (rclcpp::ok(this->context_) && spinning.load()) {
     rclcpp::executor::AnyExecutable any_executable;
     if (get_next_executable(any_executable)) {
       execute_any_executable(any_executable);

rclcpp/src/rclcpp/expand_topic_or_service_name.cpp

@@ -51,14 +51,7 @@ rclcpp::expand_topic_or_service_name(
   }
   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);
-      });
+    const rcutils_error_state_t * error_state = rcl_get_error_state();
     // finalize the string map before throwing
     rcutils_ret = rcutils_string_map_fini(&substitutions_map);
     if (rcutils_ret != RCUTILS_RET_OK) {
@@ -66,10 +59,10 @@ rclcpp::expand_topic_or_service_name(
         "rclcpp",
         "failed to fini string_map (%d) during error handling: %s",
         rcutils_ret,
-        rcutils_get_error_string_safe());
+        rcutils_get_error_string().str);
       rcutils_reset_error();
     }
-    throw_from_rcl_error(ret, "", &error_state);
+    throw_from_rcl_error(ret, "", error_state);
   }
 
   ret = rcl_expand_topic_name(

rclcpp/src/rclcpp/graph_listener.cpp

@@ -35,22 +35,32 @@ namespace rclcpp
 namespace graph_listener
 {
 
-GraphListener::GraphListener()
-: is_started_(false), is_shutdown_(false), shutdown_guard_condition_(nullptr)
+GraphListener::GraphListener(std::shared_ptr<rclcpp::Context> parent_context)
+: parent_context_(parent_context),
+  is_started_(false),
+  is_shutdown_(false),
+  interrupt_guard_condition_context_(nullptr),
+  shutdown_guard_condition_(nullptr)
 {
+  // TODO(wjwwood): make a guard condition class in rclcpp so this can be tracked
+  //   automatically with the rcl guard condition
+  // hold on to this context to prevent it from going out of scope while this
+  // guard condition is using it.
+  interrupt_guard_condition_context_ = parent_context->get_rcl_context();
   rcl_ret_t ret = rcl_guard_condition_init(
     &interrupt_guard_condition_,
+    interrupt_guard_condition_context_.get(),
     rcl_guard_condition_get_default_options());
   if (RCL_RET_OK != ret) {
     throw_from_rcl_error(ret, "failed to create interrupt guard condition");
   }
 
-  shutdown_guard_condition_ = rclcpp::get_sigint_guard_condition(&wait_set_);
+  shutdown_guard_condition_ = parent_context->get_interrupt_guard_condition(&wait_set_);
 }
 
 GraphListener::~GraphListener()
 {
-  this->shutdown();
+  this->shutdown(std::nothrow);
 }
 
 void
@@ -81,7 +91,8 @@ GraphListener::start_if_not_started()
       [weak_this]() {
         auto shared_this = weak_this.lock();
         if (shared_this) {
-          shared_this->shutdown();
+          // should not throw from on_shutdown if it can be avoided
+          shared_this->shutdown(std::nothrow);
         }
       });
     // Start the listener thread.
@@ -130,29 +141,35 @@ GraphListener::run_loop()
     std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
 
     // Resize the wait set if necessary.
-    if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_.size() + 2)) {
-      ret = rcl_wait_set_resize_guard_conditions(&wait_set_, node_graph_interfaces_.size() + 2);
+    const size_t node_graph_interfaces_size = node_graph_interfaces_.size();
+    // Add 2 for the interrupt and shutdown guard conditions
+    if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_size + 2)) {
+      ret = rcl_wait_set_resize(&wait_set_, 0, node_graph_interfaces_size + 2, 0, 0, 0);
       if (RCL_RET_OK != ret) {
         throw_from_rcl_error(ret, "failed to resize wait set");
       }
     }
-    // Clear the wait set's guard conditions.
-    ret = rcl_wait_set_clear_guard_conditions(&wait_set_);
+    // Clear the wait set.
+    ret = rcl_wait_set_clear(&wait_set_);
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to clear wait set");
     }
     // Put the interrupt guard condition in the wait set.
-    ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_);
+    ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_, NULL);
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set");
     }
     // Put the shutdown guard condition in the wait set.
-    ret = rcl_wait_set_add_guard_condition(&wait_set_, shutdown_guard_condition_);
+    size_t shutdown_guard_condition_index = 0u;
+    ret = rcl_wait_set_add_guard_condition(
+      &wait_set_, shutdown_guard_condition_, &shutdown_guard_condition_index);
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set");
     }
     // Put graph guard conditions for each node into the wait set.
-    for (const auto node_ptr : node_graph_interfaces_) {
+    std::vector<size_t> graph_gc_indexes(node_graph_interfaces_size, 0u);
+    for (size_t i = 0u; i < node_graph_interfaces_size; ++i) {
+      auto node_ptr = node_graph_interfaces_[i];
       // Only wait on graph changes if some user of the node is watching.
       if (node_ptr->count_graph_users() == 0) {
         continue;
@@ -162,7 +179,7 @@ GraphListener::run_loop()
       if (!graph_gc) {
         throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
       }
-      ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc);
+      ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc, &graph_gc_indexes[i]);
       if (RCL_RET_OK != ret) {
         throw_from_rcl_error(ret, "failed to add graph guard condition to wait set");
       }
@@ -177,23 +194,18 @@ GraphListener::run_loop()
       throw_from_rcl_error(ret, "failed to wait on wait set");
     }
 
-    bool shutdown_guard_condition_triggered = false;
     // Check to see if the shutdown guard condition has been triggered.
-    for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
-      if (shutdown_guard_condition_ == wait_set_.guard_conditions[i]) {
-        shutdown_guard_condition_triggered = true;
-      }
-    }
+    bool shutdown_guard_condition_triggered =
+      (shutdown_guard_condition_ == wait_set_.guard_conditions[shutdown_guard_condition_index]);
     // Notify nodes who's guard conditions are set (triggered).
-    for (const auto node_ptr : node_graph_interfaces_) {
+    for (size_t i = 0u; i < node_graph_interfaces_size; ++i) {
+      const auto node_ptr = node_graph_interfaces_[i];
       auto graph_gc = node_ptr->get_graph_guard_condition();
       if (!graph_gc) {
         throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
       }
-      for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
-        if (graph_gc == wait_set_.guard_conditions[i]) {
-          node_ptr->notify_graph_change();
-        }
+      if (graph_gc == wait_set_.guard_conditions[graph_gc_indexes[i]]) {
+        node_ptr->notify_graph_change();
       }
       if (shutdown_guard_condition_triggered) {
         // If shutdown, then notify the node of this as well.
@@ -327,7 +339,7 @@ GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_gr
 }
 
 void
-GraphListener::shutdown()
+GraphListener::__shutdown(bool should_throw)
 {
   std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
   if (!is_shutdown_.exchange(true)) {
@@ -336,11 +348,16 @@ GraphListener::shutdown()
       listener_thread_.join();
     }
     rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_);
+    interrupt_guard_condition_context_.reset();  // release context guard condition was using
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to finalize interrupt guard condition");
     }
     if (shutdown_guard_condition_) {
-      rclcpp::release_sigint_guard_condition(&wait_set_);
+      if (should_throw) {
+        parent_context_->release_interrupt_guard_condition(&wait_set_);
+      } else {
+        parent_context_->release_interrupt_guard_condition(&wait_set_, std::nothrow);
+      }
       shutdown_guard_condition_ = nullptr;
     }
     if (is_started_) {
@@ -352,6 +369,29 @@ GraphListener::shutdown()
   }
 }
 
+void
+GraphListener::shutdown()
+{
+  this->__shutdown(true);
+}
+
+void
+GraphListener::shutdown(const std::nothrow_t &) noexcept
+{
+  try {
+    this->__shutdown(false);
+  } catch (const std::exception & exc) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "caught %s exception when shutting down GraphListener: %s",
+      rmw::impl::cpp::demangle(exc).c_str(), exc.what());
+  } catch (...) {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rclcpp"),
+      "caught unknown exception when shutting down GraphListener");
+  }
+}
+
 bool
 GraphListener::is_shutdown()
 {

rclcpp/src/rclcpp/init_options.cpp

@@ -0,0 +1,86 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/init_options.hpp"
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
+
+namespace rclcpp
+{
+
+InitOptions::InitOptions(rcl_allocator_t allocator)
+: init_options_(new rcl_init_options_t)
+{
+  *init_options_ = rcl_get_zero_initialized_init_options();
+  rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialized rcl init options");
+  }
+}
+
+InitOptions::InitOptions(const rcl_init_options_t & init_options)
+: init_options_(new rcl_init_options_t)
+{
+  *init_options_ = rcl_get_zero_initialized_init_options();
+  rcl_ret_t ret = rcl_init_options_copy(&init_options, init_options_.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options");
+  }
+}
+
+InitOptions::InitOptions(const InitOptions & other)
+: InitOptions(*other.get_rcl_init_options())
+{}
+
+InitOptions &
+InitOptions::operator=(const InitOptions & other)
+{
+  if (this != &other) {
+    this->finalize_init_options();
+    rcl_ret_t ret = rcl_init_options_copy(other.get_rcl_init_options(), init_options_.get());
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options");
+    }
+  }
+  return *this;
+}
+
+InitOptions::~InitOptions()
+{
+  this->finalize_init_options();
+}
+
+void
+InitOptions::finalize_init_options()
+{
+  if (init_options_) {
+    rcl_ret_t ret = rcl_init_options_fini(init_options_.get());
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "failed to finalize rcl init options: %s", rcl_get_error_string().str);
+      rcl_reset_error();
+    }
+    *init_options_ = rcl_get_zero_initialized_init_options();
+  }
+}
+
+const rcl_init_options_t *
+InitOptions::get_rcl_init_options() const
+{
+  return init_options_.get();
+}
+
+}  // namespace rclcpp

rclcpp/src/rclcpp/logger.cpp

@@ -16,10 +16,12 @@
 
 #include "rclcpp/logger.hpp"
 
+#include "rclcpp/logging.hpp"
+
 namespace rclcpp
 {
 
-rclcpp::Logger
+Logger
 get_logger(const std::string & name)
 {
 #if RCLCPP_LOGGING_ENABLED
@@ -30,4 +32,16 @@ get_logger(const std::string & name)
 #endif
 }
 
+Logger
+get_node_logger(const rcl_node_t * node)
+{
+  const char * logger_name = rcl_node_get_logger_name(node);
+  if (nullptr == logger_name) {
+    auto logger = rclcpp::get_logger("rclcpp");
+    RCLCPP_ERROR(logger, "failed to get logger name from node at address %p", node);
+    return logger;
+  }
+  return rclcpp::get_logger(logger_name);
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/memory_strategy.cpp

@@ -200,3 +200,29 @@ MemoryStrategy::get_group_by_client(
   }
   return nullptr;
 }
+
+rclcpp::callback_group::CallbackGroup::SharedPtr
+MemoryStrategy::get_group_by_waitable(
+  rclcpp::Waitable::SharedPtr waitable,
+  const WeakNodeVector & weak_nodes)
+{
+  for (auto & weak_node : weak_nodes) {
+    auto node = weak_node.lock();
+    if (!node) {
+      continue;
+    }
+    for (auto & weak_group : node->get_callback_groups()) {
+      auto group = weak_group.lock();
+      if (!group) {
+        continue;
+      }
+      for (auto & weak_waitable : group->get_waitable_ptrs()) {
+        auto group_waitable = weak_waitable.lock();
+        if (group_waitable && group_waitable == waitable) {
+          return group;
+        }
+      }
+    }
+  }
+  return nullptr;
+}

rclcpp/src/rclcpp/node.cpp

@@ -29,8 +29,10 @@
 #include "rclcpp/node_interfaces/node_logging.hpp"
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 #include "rclcpp/node_interfaces/node_services.hpp"
+#include "rclcpp/node_interfaces/node_time_source.hpp"
 #include "rclcpp/node_interfaces/node_timers.hpp"
 #include "rclcpp/node_interfaces/node_topics.hpp"
+#include "rclcpp/node_interfaces/node_waitables.hpp"
 
 using rclcpp::Node;
 using rclcpp::exceptions::throw_from_rcl_error;
@@ -66,20 +68,32 @@ Node::Node(
   node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
   node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
   node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())),
-  node_parameters_(new rclcpp::node_interfaces::NodeParameters(
-      node_base_,
-      node_topics_,
-      node_services_,
-      initial_parameters,
-      use_intra_process_comms,
-      start_parameter_services
-    )),
   node_clock_(new rclcpp::node_interfaces::NodeClock(
       node_base_,
       node_topics_,
       node_graph_,
-      node_services_
+      node_services_,
+      node_logging_
     )),
+  node_parameters_(new rclcpp::node_interfaces::NodeParameters(
+      node_base_,
+      node_topics_,
+      node_services_,
+      node_clock_,
+      initial_parameters,
+      use_intra_process_comms,
+      start_parameter_services
+    )),
+  node_time_source_(new rclcpp::node_interfaces::NodeTimeSource(
+      node_base_,
+      node_topics_,
+      node_graph_,
+      node_services_,
+      node_logging_,
+      node_clock_,
+      node_parameters_
+    )),
+  node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   use_intra_process_comms_(use_intra_process_comms)
 {
 }
@@ -173,6 +187,12 @@ Node::list_parameters(
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
+std::vector<std::string>
+Node::get_node_names() const
+{
+  return node_graph_->get_node_names();
+}
+
 std::map<std::string, std::vector<std::string>>
 Node::get_topic_names_and_types() const
 {
@@ -247,6 +267,18 @@ Node::get_node_graph_interface()
   return node_graph_;
 }
 
+rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+Node::get_node_logging_interface()
+{
+  return node_logging_;
+}
+
+rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
+Node::get_node_time_source_interface()
+{
+  return node_time_source_;
+}
+
 rclcpp::node_interfaces::NodeTimersInterface::SharedPtr
 Node::get_node_timers_interface()
 {
@@ -270,3 +302,9 @@ Node::get_node_parameters_interface()
 {
   return node_parameters_;
 }
+
+rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
+Node::get_node_waitables_interface()
+{
+  return node_waitables_;
+}

rclcpp/src/rclcpp/node_interfaces/node_base.cpp

@@ -22,8 +22,8 @@
 #include "rcl/arguments.h"
 #include "rclcpp/exceptions.hpp"
 #include "rcutils/logging_macros.h"
-#include "rmw/validate_node_name.h"
 #include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
 
 using rclcpp::exceptions::throw_from_rcl_error;
 
@@ -43,7 +43,8 @@ NodeBase::NodeBase(
 {
   // Setup the guard condition that is notified when changes occur in the graph.
   rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options();
-  rcl_ret_t ret = rcl_guard_condition_init(&notify_guard_condition_, guard_condition_options);
+  rcl_ret_t ret = rcl_guard_condition_init(
+    &notify_guard_condition_, context->get_rcl_context().get(), guard_condition_options);
   if (ret != RCL_RET_OK) {
     throw_from_rcl_error(ret, "failed to create interrupt guard condition");
   }
@@ -54,7 +55,7 @@ NodeBase::NodeBase(
       if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "failed to destroy guard condition: %s", rcl_get_error_string_safe());
+          "failed to destroy guard condition: %s", rcl_get_error_string().str);
       }
     };
 
@@ -113,7 +114,10 @@ NodeBase::NodeBase(
   // TODO(wjwwood): pass the Allocator to the options
   options.domain_id = domain_id;
 
-  ret = rcl_node_init(rcl_node.get(), node_name.c_str(), namespace_.c_str(), &options);
+  ret = rcl_node_init(
+    rcl_node.get(),
+    node_name.c_str(), namespace_.c_str(),
+    context->get_rcl_context().get(), &options);
   if (ret != RCL_RET_OK) {
     // Finalize the interrupt guard condition.
     finalize_notify_guard_condition();
@@ -122,7 +126,7 @@ NodeBase::NodeBase(
       // Print message because exception will be thrown later in this code block
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
-        "Failed to fini arguments during error handling: %s", rcl_get_error_string_safe());
+        "Failed to fini arguments during error handling: %s", rcl_get_error_string().str);
       rcl_reset_error();
     }
 
@@ -180,7 +184,7 @@ NodeBase::NodeBase(
       if (rcl_node_fini(node) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "Error in destruction of rcl node handle: %s", rcl_get_error_string_safe());
+          "Error in destruction of rcl node handle: %s", rcl_get_error_string().str);
       }
       delete node;
     });
@@ -197,7 +201,7 @@ NodeBase::NodeBase(
     // print message because throwing would prevent the destructor from being called
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
-      "Failed to fini arguments: %s", rcl_get_error_string_safe());
+      "Failed to fini arguments: %s", rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -211,7 +215,7 @@ NodeBase::~NodeBase()
     if (rcl_guard_condition_fini(&notify_guard_condition_) != RCL_RET_OK) {
       RCUTILS_LOG_ERROR_NAMED(
         "rclcpp",
-        "failed to destroy guard condition: %s", rcl_get_error_string_safe());
+        "failed to destroy guard condition: %s", rcl_get_error_string().str);
     }
   }
 }

rclcpp/src/rclcpp/node_interfaces/node_clock.cpp

@@ -23,20 +23,15 @@ NodeClock::NodeClock(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
-  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services)
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging)
 : node_base_(node_base),
   node_topics_(node_topics),
   node_graph_(node_graph),
   node_services_(node_services),
+  node_logging_(node_logging),
   ros_clock_(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME))
-{
-  time_source_.attachNode(
-    node_base_,
-    node_topics_,
-    node_graph_,
-    node_services_);
-  time_source_.attachClock(ros_clock_);
-}
+{}
 
 NodeClock::~NodeClock()
 {}

rclcpp/src/rclcpp/node_interfaces/node_graph.cpp

@@ -16,6 +16,7 @@
 
 #include <map>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl/graph.h"
@@ -29,7 +30,9 @@ using rclcpp::graph_listener::GraphListener;
 
 NodeGraph::NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base)
 : node_base_(node_base),
-  graph_listener_(node_base->get_context()->get_sub_context<GraphListener>()),
+  graph_listener_(
+    node_base->get_context()->get_sub_context<GraphListener>(node_base->get_context())
+  ),
   should_add_to_graph_listener_(true),
   graph_users_count_(0)
 {}
@@ -58,13 +61,13 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const
     &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_get_error_string().str;
     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();
+        rcl_get_error_string().str;
     }
-    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+    throw std::runtime_error(error_msg + rcl_get_error_string().str);
   }
 
   std::map<std::string, std::vector<std::string>> topics_and_types;
@@ -79,7 +82,7 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const
   if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not destroy topic names and types: ") + rcl_get_error_string_safe());
+      std::string("could not destroy topic names and types: ") + rcl_get_error_string().str);
     // *INDENT-ON*
   }
 
@@ -98,14 +101,14 @@ NodeGraph::get_service_names_and_types() const
     &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_get_error_string().str;
     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();
+        rcl_get_error_string().str;
     }
-    throw std::runtime_error(error_msg + rcl_get_error_string_safe());
+    throw std::runtime_error(error_msg + rcl_get_error_string().str);
   }
 
   std::map<std::string, std::vector<std::string>> services_and_types;
@@ -120,7 +123,7 @@ NodeGraph::get_service_names_and_types() const
   if (ret != RCL_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not destroy service names and types: ") + rcl_get_error_string_safe());
+      std::string("could not destroy service names and types: ") + rcl_get_error_string().str);
     // *INDENT-ON*
   }
 
@@ -129,40 +132,73 @@ NodeGraph::get_service_names_and_types() const
 
 std::vector<std::string>
 NodeGraph::get_node_names() const
+{
+  std::vector<std::string> nodes;
+  auto names_and_namespaces = get_node_names_and_namespaces();
+
+  for (const auto & it : names_and_namespaces) {
+    nodes.push_back(it.first);
+  }
+  return nodes;
+}
+
+std::vector<std::pair<std::string, std::string>>
+NodeGraph::get_node_names_and_namespaces() const
 {
   rcutils_string_array_t node_names_c =
     rcutils_get_zero_initialized_string_array();
+  rcutils_string_array_t node_namespaces_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);
+    &node_names_c,
+    &node_namespaces_c);
   if (ret != RCL_RET_OK) {
-    auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string_safe();
+    auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string().str;
     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();
+        rcl_get_error_string().str;
+      rcl_reset_error();
+    }
+    if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) {
+      error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") +
+        rcl_get_error_string().str;
+      rcl_reset_error();
     }
     // 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.size);
+  std::vector<std::pair<std::string, std::string>> node_names(node_names_c.size);
   for (size_t i = 0; i < node_names_c.size; ++i) {
-    if (node_names_c.data[i]) {
-      node_names[i] = node_names_c.data[i];
+    if (node_names_c.data[i] && node_namespaces_c.data[i]) {
+      node_names.push_back(std::make_pair(node_names_c.data[i], node_namespaces_c.data[i]));
     }
   }
-  ret = rcutils_string_array_fini(&node_names_c);
-  if (ret != RCUTILS_RET_OK) {
+
+  std::string error;
+  rcl_ret_t ret_names = rcutils_string_array_fini(&node_names_c);
+  if (ret_names != 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: "));
+    error = "could not destroy node names";
     // *INDENT-ON*
   }
+  rcl_ret_t ret_ns = rcutils_string_array_fini(&node_namespaces_c);
+  if (ret_ns != RCUTILS_RET_OK) {
+    // *INDENT-OFF*
+    // TODO(karsten1987): Append rcutils_error_message once it's in master
+    error += ", could not destroy node namespaces";
+    // *INDENT-ON*
+  }
+
+  if (ret_names != RCUTILS_RET_OK || ret_ns != RCUTILS_RET_OK) {
+    throw std::runtime_error(error);
+  }
 
   return node_names;
 }
@@ -183,7 +219,7 @@ NodeGraph::count_publishers(const std::string & topic_name) const
   if (ret != RMW_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not count publishers: ") + rmw_get_error_string_safe());
+      std::string("could not count publishers: ") + rmw_get_error_string().str);
     // *INDENT-ON*
   }
   return count;
@@ -205,7 +241,7 @@ NodeGraph::count_subscribers(const std::string & topic_name) const
   if (ret != RMW_RET_OK) {
     // *INDENT-OFF*
     throw std::runtime_error(
-      std::string("could not count subscribers: ") + rmw_get_error_string_safe());
+      std::string("could not count subscribers: ") + rmw_get_error_string().str);
     // *INDENT-ON*
   }
   return count;
@@ -300,8 +336,8 @@ NodeGraph::wait_for_graph_change(
       throw EventNotRegisteredError();
     }
   }
-  auto pred = [&event]() {
-      return event->check() || !rclcpp::ok();
+  auto pred = [&event, context = node_base_->get_context()]() {
+      return event->check() || !rclcpp::ok(context);
     };
   std::unique_lock<std::mutex> graph_lock(graph_mutex_);
   if (!pred()) {

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -18,6 +18,7 @@
 
 #include <map>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <utility>
 #include <vector>
@@ -35,9 +36,11 @@ NodeParameters::NodeParameters(
   const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
   const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
   const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+  const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
   const std::vector<rclcpp::Parameter> & initial_parameters,
   bool use_intra_process,
   bool start_parameter_services)
+: node_clock_(node_clock)
 {
   using MessageT = rcl_interfaces::msg::ParameterEvent;
   using PublisherT = rclcpp::Publisher<MessageT>;
@@ -63,7 +66,7 @@ NodeParameters::NodeParameters(
   }
   const rcl_node_options_t * options = rcl_node_get_options(node);
   if (nullptr == options) {
-    throw std::runtime_error("Need valid node options NodeParameters");
+    throw std::runtime_error("Need valid node options in NodeParameters");
   }
 
   // Get paths to yaml files containing initial parameter values
@@ -92,7 +95,8 @@ NodeParameters::NodeParameters(
 
   // global before local so that local overwrites global
   if (options->use_global_arguments) {
-    get_yaml_paths(rcl_get_global_arguments());
+    auto context_ptr = node_base->get_context()->get_rcl_context();
+    get_yaml_paths(&(context_ptr->global_arguments));
   }
   get_yaml_paths(&(options->arguments));
 
@@ -103,28 +107,33 @@ NodeParameters::NodeParameters(
     // Should never happen
     throw std::runtime_error("Node name and namespace were not set");
   }
-  std::string combined_name;
+
   if ('/' == node_namespace.at(node_namespace.size() - 1)) {
-    combined_name = node_namespace + node_name;
+    combined_name_ = node_namespace + node_name;
   } else {
-    combined_name = node_namespace + '/' + node_name;
+    combined_name_ = node_namespace + '/' + node_name;
   }
 
   std::map<std::string, rclcpp::Parameter> parameters;
 
   // TODO(sloretz) use rcl to parse yaml when circular dependency is solved
+  // See https://github.com/ros2/rcl/issues/252
   for (const std::string & yaml_path : yaml_paths) {
     rcl_params_t * yaml_params = rcl_yaml_node_struct_init(options->allocator);
     if (nullptr == yaml_params) {
       throw std::bad_alloc();
     }
     if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params)) {
-      throw std::runtime_error("Failed to parse parameters " + yaml_path);
+      std::ostringstream ss;
+      ss << "Failed to parse parameters from file '" << yaml_path << "': " <<
+        rcl_get_error_string().str;
+      rcl_reset_error();
+      throw std::runtime_error(ss.str());
     }
 
     rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(yaml_params);
     rcl_yaml_node_struct_fini(yaml_params);
-    auto iter = initial_map.find(combined_name);
+    auto iter = initial_map.find(combined_name_);
     if (initial_map.end() == iter) {
       continue;
     }
@@ -179,6 +188,8 @@ NodeParameters::set_parameters_atomically(
   std::map<std::string, rclcpp::Parameter> tmp_map;
   auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
 
+  parameter_event->node = combined_name_;
+
   // TODO(jacquelinekay): handle parameter constraints
   rcl_interfaces::msg::SetParametersResult result;
   if (parameters_callback_) {
@@ -221,7 +232,7 @@ NodeParameters::set_parameters_atomically(
   }
 
   std::swap(tmp_map, parameters_);
-
+  parameter_event->stamp = node_clock_->get_clock()->now();
   events_publisher_->publish(parameter_event);
 
   return result;
@@ -356,7 +367,7 @@ NodeParameters::register_param_change_callback(ParametersCallbackFunction callba
 {
   if (parameters_callback_) {
     RCUTILS_LOG_WARN("param_change_callback already registered, "
-      "overwriting previous callback")
+      "overwriting previous callback");
   }
   parameters_callback_ = callback;
 }

rclcpp/src/rclcpp/node_interfaces/node_services.cpp

@@ -45,8 +45,8 @@ NodeServices::add_service(
     auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
     if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
       throw std::runtime_error(
-              std::string(
-                "Failed to notify wait set on service creation: ") + rmw_get_error_string()
+              std::string("Failed to notify wait set on service creation: ") +
+              rmw_get_error_string().str
       );
     }
   }
@@ -72,7 +72,8 @@ NodeServices::add_client(
     auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
     if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
       throw std::runtime_error(
-              std::string("Failed to notify wait set on client creation: ") + rmw_get_error_string()
+              std::string("Failed to notify wait set on client creation: ") +
+              rmw_get_error_string().str
       );
     }
   }

rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp

@@ -0,0 +1,50 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/node_interfaces/node_time_source.hpp"
+
+#include <memory>
+#include <string>
+
+using rclcpp::node_interfaces::NodeTimeSource;
+
+NodeTimeSource::NodeTimeSource(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters)
+: node_base_(node_base),
+  node_topics_(node_topics),
+  node_graph_(node_graph),
+  node_services_(node_services),
+  node_logging_(node_logging),
+  node_clock_(node_clock),
+  node_parameters_(node_parameters)
+{
+  time_source_.attachNode(
+    node_base_,
+    node_topics_,
+    node_graph_,
+    node_services_,
+    node_logging_,
+    node_clock_,
+    node_parameters_);
+  time_source_.attachClock(node_clock_->get_clock());
+}
+
+NodeTimeSource::~NodeTimeSource()
+{}

rclcpp/src/rclcpp/node_interfaces/node_timers.cpp

@@ -41,7 +41,7 @@ NodeTimers::add_timer(
   }
   if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
     throw std::runtime_error(
-            std::string(
-              "Failed to notify wait set on timer creation: ") + rmw_get_error_string());
+            std::string("Failed to notify wait set on timer creation: ") +
+            rmw_get_error_string().str);
   }
 }

rclcpp/src/rclcpp/node_interfaces/node_topics.cpp

@@ -73,8 +73,8 @@ NodeTopics::add_publisher(
     auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
     if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
       throw std::runtime_error(
-              std::string(
-                "Failed to notify wait set on publisher creation: ") + rmw_get_error_string());
+              std::string("Failed to notify wait set on publisher creation: ") +
+              rmw_get_error_string().str);
     }
   }
 }
@@ -123,8 +123,8 @@ NodeTopics::add_subscription(
     auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
     if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
       throw std::runtime_error(
-              std::string(
-                "Failed to notify wait set on subscription creation: ") + rmw_get_error_string()
+              std::string("Failed to notify wait set on subscription creation: ") +
+              rmw_get_error_string().str
       );
     }
   }

rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp

@@ -0,0 +1,68 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/node_interfaces/node_waitables.hpp"
+
+#include <string>
+
+using rclcpp::node_interfaces::NodeWaitables;
+
+NodeWaitables::NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base)
+: node_base_(node_base)
+{}
+
+NodeWaitables::~NodeWaitables()
+{}
+
+void
+NodeWaitables::add_waitable(
+  rclcpp::Waitable::SharedPtr waitable_ptr,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  if (group) {
+    if (!node_base_->callback_group_in_node(group)) {
+      // TODO(jacobperron): use custom exception
+      throw std::runtime_error("Cannot create waitable, group not in node.");
+    }
+    group->add_waitable(waitable_ptr);
+  } else {
+    node_base_->get_default_callback_group()->add_waitable(waitable_ptr);
+  }
+
+  // Notify the executor that a new waitable was created using the parent Node.
+  {
+    auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock();
+    if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) {
+      throw std::runtime_error(
+              std::string("Failed to notify wait set on waitable creation: ") +
+              rmw_get_error_string().str
+      );
+    }
+  }
+}
+
+void
+NodeWaitables::remove_waitable(
+  rclcpp::Waitable::SharedPtr waitable_ptr,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group) noexcept
+{
+  if (group) {
+    if (!node_base_->callback_group_in_node(group)) {
+      return;
+    }
+    group->remove_waitable(waitable_ptr);
+  } else {
+    node_base_->get_default_callback_group()->remove_waitable(waitable_ptr);
+  }
+}

rclcpp/src/rclcpp/publisher.cpp

@@ -66,12 +66,12 @@ PublisherBase::PublisherBase(
   // 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) {
-    auto msg = 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().str;
     rcl_reset_error();
     throw std::runtime_error(msg);
   }
   if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) {
-    auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string_safe();
+    auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string().str;
     rmw_reset_error();
     throw std::runtime_error(msg);
   }
@@ -83,7 +83,7 @@ PublisherBase::~PublisherBase()
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
       "Error in destruction of intra process rcl publisher handle: %s",
-      rcl_get_error_string_safe());
+      rcl_get_error_string().str);
     rcl_reset_error();
   }
 
@@ -91,7 +91,7 @@ PublisherBase::~PublisherBase()
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
       "Error in destruction of rcl publisher handle: %s",
-      rcl_get_error_string_safe());
+      rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -107,7 +107,7 @@ PublisherBase::get_queue_size() const
 {
   const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_);
   if (!publisher_options) {
-    auto msg = 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().str;
     rcl_reset_error();
     throw std::runtime_error(msg);
   }
@@ -150,14 +150,14 @@ 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) {
-    auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe();
+    auto msg = std::string("failed to compare gids: ") + rmw_get_error_string().str;
     rmw_reset_error();
     throw std::runtime_error(msg);
   }
   if (!result) {
     ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result);
     if (ret != RMW_RET_OK) {
-      auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe();
+      auto msg = std::string("failed to compare gids: ") + rmw_get_error_string().str;
       rmw_reset_error();
       throw std::runtime_error(msg);
     }
@@ -204,7 +204,7 @@ PublisherBase::setup_intra_process(
   rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(
     &intra_process_publisher_handle_);
   if (publisher_rmw_handle == nullptr) {
-    auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string_safe();
+    auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string().str;
     rcl_reset_error();
     throw std::runtime_error(msg);
   }
@@ -212,7 +212,7 @@ PublisherBase::setup_intra_process(
     publisher_rmw_handle, &intra_process_rmw_gid_);
   if (rmw_ret != RMW_RET_OK) {
     auto msg =
-      std::string("failed to create intra process publisher gid: ") + rmw_get_error_string_safe();
+      std::string("failed to create intra process publisher gid: ") + rmw_get_error_string().str;
     rmw_reset_error();
     throw std::runtime_error(msg);
   }

rclcpp/src/rclcpp/signal_handler.cpp

@@ -0,0 +1,357 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "./signal_handler.hpp"
+
+#include <atomic>
+#include <csignal>
+#include <mutex>
+#include <string>
+#include <thread>
+
+// includes for semaphore notification code
+#if defined(_WIN32)
+#include <windows.h>
+#elif defined(__APPLE__)
+#include <dispatch/dispatch.h>
+#else  // posix
+#include <semaphore.h>
+#endif
+
+#include "rclcpp/logging.hpp"
+#include "rmw/impl/cpp/demangle.hpp"
+
+using rclcpp::SignalHandler;
+
+// initialize static storage in SignalHandler class
+SignalHandler::signal_handler_type SignalHandler::old_signal_handler_;
+std::atomic_bool SignalHandler::signal_received_ = ATOMIC_VAR_INIT(false);
+std::atomic_bool SignalHandler::wait_for_signal_is_setup_ = ATOMIC_VAR_INIT(false);
+#if defined(_WIN32)
+HANDLE SignalHandler::signal_handler_sem_;
+#elif defined(__APPLE__)
+dispatch_semaphore_t SignalHandler::signal_handler_sem_;
+#else  // posix
+sem_t SignalHandler::signal_handler_sem_;
+#endif
+
+SignalHandler &
+SignalHandler::get_global_signal_handler()
+{
+  static SignalHandler signal_handler;
+  return signal_handler;
+}
+
+rclcpp::Logger &
+SignalHandler::get_logger()
+{
+  static rclcpp::Logger logger = rclcpp::get_logger("rclcpp");
+  return logger;
+}
+
+bool
+SignalHandler::install()
+{
+  std::lock_guard<std::mutex> lock(install_mutex_);
+  bool already_installed = installed_.exchange(true);
+  if (already_installed) {
+    return false;
+  }
+  try {
+    setup_wait_for_signal();
+    signal_received_.store(false);
+
+    SignalHandler::signal_handler_type signal_handler_argument;
+#if defined(RCLCPP_HAS_SIGACTION)
+    memset(&signal_handler_argument, 0, sizeof(signal_handler_argument));
+    sigemptyset(&signal_handler_argument.sa_mask);
+    signal_handler_argument.sa_sigaction = signal_handler;
+    signal_handler_argument.sa_flags = SA_SIGINFO;
+#else
+    signal_handler_argument = signal_handler;
+#endif
+
+    old_signal_handler_ = SignalHandler::set_signal_handler(SIGINT, signal_handler_argument);
+
+    signal_handler_thread_ = std::thread(&SignalHandler::deferred_signal_handler, this);
+  } catch (...) {
+    installed_.store(false);
+    throw;
+  }
+  RCLCPP_DEBUG(get_logger(), "signal handler installed");
+  return true;
+}
+
+bool
+SignalHandler::uninstall()
+{
+  std::lock_guard<std::mutex> lock(install_mutex_);
+  bool installed = installed_.exchange(false);
+  if (!installed) {
+    return false;
+  }
+  try {
+    // TODO(wjwwood): what happens if someone overrides our signal handler then calls uninstall?
+    //   I think we need to assert that we're the current signal handler, and mitigate if not.
+    set_signal_handler(SIGINT, old_signal_handler_);
+    RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler");
+    notify_signal_handler();
+    signal_handler_thread_.join();
+    teardown_wait_for_signal();
+  } catch (...) {
+    installed_.exchange(true);
+    throw;
+  }
+  RCLCPP_DEBUG(get_logger(), "signal handler uninstalled");
+  return true;
+}
+
+bool
+SignalHandler::is_installed()
+{
+  return installed_.load();
+}
+
+SignalHandler::~SignalHandler()
+{
+  try {
+    uninstall();
+  } catch (const std::exception & exc) {
+    RCLCPP_ERROR(
+      get_logger(),
+      "caught %s exception when uninstalling the sigint handler in rclcpp::~SignalHandler: %s",
+      rmw::impl::cpp::demangle(exc).c_str(), exc.what());
+  } catch (...) {
+    RCLCPP_ERROR(
+      get_logger(),
+      "caught unknown exception when uninstalling the sigint handler in rclcpp::~SignalHandler");
+  }
+}
+
+RCLCPP_LOCAL
+void
+__safe_strerror(int errnum, char * buffer, size_t buffer_length)
+{
+#if defined(_WIN32)
+  strerror_s(buffer, buffer_length, errnum);
+#elif (defined(_GNU_SOURCE) && !defined(ANDROID))
+  char * msg = strerror_r(errnum, buffer, buffer_length);
+  if (msg != buffer) {
+    strncpy(buffer, msg, buffer_length);
+    buffer[buffer_length - 1] = '\0';
+  }
+#else
+  int error_status = strerror_r(errnum, buffer, buffer_length);
+  if (error_status != 0) {
+    throw std::runtime_error("Failed to get error string for errno: " + std::to_string(errnum));
+  }
+#endif
+}
+
+SignalHandler::signal_handler_type
+SignalHandler::set_signal_handler(
+  int signal_value,
+  const SignalHandler::signal_handler_type & signal_handler)
+{
+  bool signal_handler_install_failed;
+  SignalHandler::signal_handler_type old_signal_handler;
+#if defined(RCLCPP_HAS_SIGACTION)
+  ssize_t ret = sigaction(signal_value, &signal_handler, &old_signal_handler);
+  signal_handler_install_failed = (ret == -1);
+#else
+  old_signal_handler = std::signal(signal_value, signal_handler);
+  signal_handler_install_failed = (old_signal_handler == SIG_ERR);
+#endif
+  if (signal_handler_install_failed) {
+    char error_string[1024];
+    __safe_strerror(errno, error_string, sizeof(error_string));
+    auto msg =
+      "Failed to set SIGINT signal handler (" + std::to_string(errno) + "): " + error_string;
+    throw std::runtime_error(msg);
+  }
+
+  return old_signal_handler;
+}
+
+void
+SignalHandler::signal_handler_common()
+{
+  signal_received_.store(true);
+  RCLCPP_DEBUG(
+    get_logger(),
+    "signal_handler(): SIGINT received, notifying deferred signal handler");
+  notify_signal_handler();
+}
+
+#if defined(RCLCPP_HAS_SIGACTION)
+void
+SignalHandler::signal_handler(int signal_value, siginfo_t * siginfo, void * context)
+{
+  RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value);
+
+  if (old_signal_handler_.sa_flags & SA_SIGINFO) {
+    if (old_signal_handler_.sa_sigaction != NULL) {
+      old_signal_handler_.sa_sigaction(signal_value, siginfo, context);
+    }
+  } else {
+    if (
+      old_signal_handler_.sa_handler != NULL &&  // Is set
+      old_signal_handler_.sa_handler != SIG_DFL &&  // Is not default
+      old_signal_handler_.sa_handler != SIG_IGN)  // Is not ignored
+    {
+      old_signal_handler_.sa_handler(signal_value);
+    }
+  }
+
+  signal_handler_common();
+}
+#else
+void
+SignalHandler::signal_handler(int signal_value)
+{
+  RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value);
+
+  if (old_signal_handler_) {
+    old_signal_handler_(signal_value);
+  }
+
+  signal_handler_common();
+}
+#endif
+
+void
+SignalHandler::deferred_signal_handler()
+{
+  while (true) {
+    if (signal_received_.exchange(false)) {
+      RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): SIGINT received, shutting down");
+      for (auto context_ptr : rclcpp::get_contexts()) {
+        if (context_ptr->get_init_options().shutdown_on_sigint) {
+          RCLCPP_DEBUG(
+            get_logger(),
+            "deferred_signal_handler(): "
+            "shutting down rclcpp::Context @ %p, because it had shutdown_on_sigint == true",
+            context_ptr.get());
+          context_ptr->shutdown("signal handler");
+        }
+      }
+    }
+    if (!is_installed()) {
+      RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): signal handling uninstalled");
+      break;
+    }
+    RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): waiting for SIGINT or uninstall");
+    wait_for_signal();
+    RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): woken up due to SIGINT or uninstall");
+  }
+}
+
+void
+SignalHandler::setup_wait_for_signal()
+{
+#if defined(_WIN32)
+  signal_handler_sem_ = CreateSemaphore(
+    NULL,  // default security attributes
+    0,  // initial semaphore count
+    1,  // maximum semaphore count
+    NULL);  // unnamed semaphore
+  if (NULL == signal_handler_sem_) {
+    throw std::runtime_error("CreateSemaphore() failed in setup_wait_for_signal()");
+  }
+#elif defined(__APPLE__)
+  signal_handler_sem_ = dispatch_semaphore_create(0);
+#else  // posix
+  if (-1 == sem_init(&signal_handler_sem_, 0, 0)) {
+    throw std::runtime_error(std::string("sem_init() failed: ") + strerror(errno));
+  }
+#endif
+  wait_for_signal_is_setup_.store(true);
+}
+
+void
+SignalHandler::teardown_wait_for_signal() noexcept
+{
+  if (!wait_for_signal_is_setup_.exchange(false)) {
+    return;
+  }
+#if defined(_WIN32)
+  CloseHandle(signal_handler_sem_);
+#elif defined(__APPLE__)
+  dispatch_release(signal_handler_sem_);
+#else  // posix
+  if (-1 == sem_destroy(&signal_handler_sem_)) {
+    RCLCPP_ERROR(get_logger(), "invalid semaphore in teardown_wait_for_signal()");
+  }
+#endif
+}
+
+void
+SignalHandler::wait_for_signal()
+{
+  if (!wait_for_signal_is_setup_.load()) {
+    RCLCPP_ERROR(get_logger(), "called wait_for_signal() before setup_wait_for_signal()");
+    return;
+  }
+#if defined(_WIN32)
+  DWORD dw_wait_result = WaitForSingleObject(signal_handler_sem_, INFINITE);
+  switch (dw_wait_result) {
+    case WAIT_ABANDONED:
+      RCLCPP_ERROR(
+        get_logger(), "WaitForSingleObject() failed in wait_for_signal() with WAIT_ABANDONED: %s",
+        GetLastError());
+      break;
+    case WAIT_OBJECT_0:
+      // successful
+      break;
+    case WAIT_TIMEOUT:
+      RCLCPP_ERROR(get_logger(), "WaitForSingleObject() timedout out in wait_for_signal()");
+      break;
+    case WAIT_FAILED:
+      RCLCPP_ERROR(
+        get_logger(), "WaitForSingleObject() failed in wait_for_signal(): %s", GetLastError());
+      break;
+    default:
+      RCLCPP_ERROR(
+        get_logger(), "WaitForSingleObject() gave unknown return in wait_for_signal(): %s",
+        GetLastError());
+  }
+#elif defined(__APPLE__)
+  dispatch_semaphore_wait(signal_handler_sem_, DISPATCH_TIME_FOREVER);
+#else  // posix
+  int s;
+  do {
+    s = sem_wait(&signal_handler_sem_);
+  } while (-1 == s && EINTR == errno);
+#endif
+}
+
+void
+SignalHandler::notify_signal_handler() noexcept
+{
+  if (!wait_for_signal_is_setup_.load()) {
+    return;
+  }
+#if defined(_WIN32)
+  if (!ReleaseSemaphore(signal_handler_sem_, 1, NULL)) {
+    RCLCPP_ERROR(
+      get_logger(), "ReleaseSemaphore() failed in notify_signal_handler(): %s", GetLastError());
+  }
+#elif defined(__APPLE__)
+  dispatch_semaphore_signal(signal_handler_sem_);
+#else  // posix
+  if (-1 == sem_post(&signal_handler_sem_)) {
+    RCLCPP_ERROR(get_logger(), "sem_post failed in notify_signal_handler()");
+  }
+#endif
+}

rclcpp/src/rclcpp/signal_handler.hpp

@@ -0,0 +1,189 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__SIGNAL_HANDLER_HPP_
+#define RCLCPP__SIGNAL_HANDLER_HPP_
+
+#include <atomic>
+#include <csignal>
+#include <mutex>
+#include <thread>
+
+#include "rclcpp/logging.hpp"
+
+// includes for semaphore notification code
+#if defined(_WIN32)
+#include <windows.h>
+#elif defined(__APPLE__)
+#include <dispatch/dispatch.h>
+#else  // posix
+#include <semaphore.h>
+#endif
+
+// Determine if sigaction is available
+#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
+#define RCLCPP_HAS_SIGACTION
+#endif
+
+namespace rclcpp
+{
+
+/// Responsible for manaaging the SIGINT signal handling.
+/**
+ * This class is responsible for:
+ *
+ * - installing the signal handler for SIGINT
+ * - uninstalling the signal handler for SIGINT
+ * - creating a thread to execute "on sigint" work outside of the signal handler
+ * - safely notifying the dedicated signal handling thread when receiving SIGINT
+ * - implementation of all of the signal handling work, like shutting down contexts
+ *
+ * \internal
+ */
+class SignalHandler final
+{
+public:
+  /// Return the global singleton of this class.
+  static
+  SignalHandler &
+  get_global_signal_handler();
+
+  /// Return a global singleton logger to avoid needing to create it everywhere.
+  static
+  rclcpp::Logger &
+  get_logger();
+
+  /// Install the signal handler for SIGINT and start the dedicated signal handling thread.
+  /**
+   * Also stores the current signal handler to be called on SIGINT and to
+   * restore when uninstalling this signal handler.
+   */
+  bool
+  install();
+
+  /// Uninstall the signal handler for SIGINT and join the dedicated singal handling thread.
+  /**
+   * Also restores the previous signal handler.
+   */
+  bool
+  uninstall();
+
+  /// Return true if installed, false otherwise.
+  bool
+  is_installed();
+
+private:
+  SignalHandler() = default;
+
+  ~SignalHandler();
+
+#if defined(RCLCPP_HAS_SIGACTION)
+  using signal_handler_type = struct sigaction;
+#else
+  using signal_handler_type = void (*)(int);
+#endif
+  // POSIX signal handler structure storage for the existing signal handler.
+  static SignalHandler::signal_handler_type old_signal_handler_;
+
+  /// Set the signal handler function.
+  static
+  SignalHandler::signal_handler_type
+  set_signal_handler(int signal_value, const SignalHandler::signal_handler_type & signal_handler);
+
+  /// Common signal handler code between sigaction and non-sigaction versions.
+  static
+  void
+  signal_handler_common();
+
+#if defined(RCLCPP_HAS_SIGACTION)
+  /// Signal handler function.
+  static
+  void
+  signal_handler(int signal_value, siginfo_t * siginfo, void * context);
+#else
+  /// Signal handler function.
+  static
+  void
+  signal_handler(int signal_value);
+#endif
+
+  /// Target of the dedicated signal handling thread.
+  void
+  deferred_signal_handler();
+
+  /// Setup anything that is necessary for wait_for_signal() or notify_signal_handler().
+  /**
+   * This must be called before wait_for_signal() or notify_signal_handler().
+   * This is not thread-safe.
+   */
+  static
+  void
+  setup_wait_for_signal();
+
+  /// Undo all setup done in setup_wait_for_signal().
+  /**
+   * Must not call wait_for_signal() or notify_signal_handler() after calling this.
+   *
+   * This is not thread-safe.
+   */
+  static
+  void
+  teardown_wait_for_signal() noexcept;
+
+  /// Wait for a notification from notify_signal_handler() in a signal safe way.
+  /**
+   * This static method may throw if posting the semaphore fails.
+   *
+   * This is not thread-safe.
+   */
+  static
+  void
+  wait_for_signal();
+
+  /// Notify blocking wait_for_signal() calls in a signal safe way.
+  /**
+   * This is used to notify the deferred_signal_handler() thread to start work
+   * from the signal handler.
+   *
+   * This is thread-safe.
+   */
+  static
+  void
+  notify_signal_handler() noexcept;
+
+  // Whether or not a signal has been received.
+  static std::atomic_bool signal_received_;
+  // A thread to which singal handling tasks are deferred.
+  std::thread signal_handler_thread_;
+
+  // A mutex used to synchronize the install() and uninstall() methods.
+  std::mutex install_mutex_;
+  // Whether or not the signal handler has been installed.
+  std::atomic_bool installed_{false};
+
+  // Whether or not the semaphore for wait_for_signal is setup.
+  static std::atomic_bool wait_for_signal_is_setup_;
+  // Storage for the wait_for_signal semaphore.
+#if defined(_WIN32)
+  static HANDLE signal_handler_sem_;
+#elif defined(__APPLE__)
+  static dispatch_semaphore_t signal_handler_sem_;
+#else  // posix
+  static sem_t signal_handler_sem_;
+#endif
+};
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__SIGNAL_HANDLER_HPP_

rclcpp/src/rclcpp/subscription.cpp

@@ -37,23 +37,14 @@ SubscriptionBase::SubscriptionBase(
   type_support_(type_support_handle),
   is_serialized_(is_serialized)
 {
-  std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
-  auto custom_deletor = [weak_node_handle](rcl_subscription_t * rcl_subs)
+  auto custom_deletor = [node_handle](rcl_subscription_t * rcl_subs)
     {
-      auto handle = weak_node_handle.lock();
-      if (handle) {
-        if (rcl_subscription_fini(rcl_subs, handle.get()) != RCL_RET_OK) {
-          RCLCPP_ERROR(
-            rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
-            "Error in destruction of rcl subscription handle: %s",
-            rcl_get_error_string_safe());
-          rcl_reset_error();
-        }
-      } else {
+      if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) {
         RCLCPP_ERROR(
-          rclcpp::get_logger("rclcpp"),
-          "Error in destruction of rcl subscription handle: "
-          "the Node Handle was destructed too early. You will leak memory");
+          rclcpp::get_node_logger(node_handle.get()).get_child("rclcpp"),
+          "Error in destruction of rcl subscription handle: %s",
+          rcl_get_error_string().str);
+        rcl_reset_error();
       }
       delete rcl_subs;
     };

rclcpp/src/rclcpp/time.cpp

@@ -227,6 +227,12 @@ Time::nanoseconds() const
   return rcl_time_.nanoseconds;
 }
 
+double
+Time::seconds() const
+{
+  return std::chrono::duration<double>(std::chrono::nanoseconds(rcl_time_.nanoseconds)).count();
+}
+
 rcl_clock_type_t
 Time::get_clock_type() const
 {
@@ -245,5 +251,11 @@ operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs)
   return Time(lhs.nanoseconds() + rhs.nanoseconds(), rhs.get_clock_type());
 }
 
+Time
+Time::max()
+{
+  return Time(std::numeric_limits<int32_t>::max(), 999999999);
+}
+
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/time_source.cpp

@@ -21,10 +21,9 @@
 
 #include "rcl/time.h"
 
-#include "rcutils/logging_macros.h"
-
 #include "rclcpp/clock.hpp"
 #include "rclcpp/exceptions.hpp"
+#include "rclcpp/logging.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/time_source.hpp"
@@ -34,13 +33,16 @@ namespace rclcpp
 {
 
 TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
-: ros_time_active_(false)
+: logger_(rclcpp::get_logger("rclcpp")),
+  clock_subscription_(nullptr),
+  ros_time_active_(false)
 {
   this->attachNode(node);
 }
 
 TimeSource::TimeSource()
-: ros_time_active_(false)
+: logger_(rclcpp::get_logger("rclcpp")),
+  ros_time_active_(false)
 {
 }
 
@@ -50,49 +52,55 @@ void TimeSource::attachNode(rclcpp::Node::SharedPtr node)
     node->get_node_base_interface(),
     node->get_node_topics_interface(),
     node->get_node_graph_interface(),
-    node->get_node_services_interface());
+    node->get_node_services_interface(),
+    node->get_node_logging_interface(),
+    node->get_node_clock_interface(),
+    node->get_node_parameters_interface());
 }
 
 void TimeSource::attachNode(
-  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)
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+  rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface)
 {
   node_base_ = node_base_interface;
   node_topics_ = node_topics_interface;
   node_graph_ = node_graph_interface;
   node_services_ = node_services_interface;
+  node_logging_ = node_logging_interface;
+  node_clock_ = node_clock_interface;
+  node_parameters_ = node_parameters_interface;
   // TODO(tfoote): Update QOS
 
-  const std::string & topic_name = "/clock";
+  logger_ = node_logging_->get_logger();
 
-  rclcpp::callback_group::CallbackGroup::SharedPtr group;
-  using rclcpp::message_memory_strategy::MessageMemoryStrategy;
-  auto msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
-  auto allocator = std::make_shared<Alloc>();
-
-  auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1);
-
-  clock_subscription_ = rclcpp::create_subscription<
-    MessageT, decltype(cb), Alloc, MessageT, SubscriptionT
-    >(
-    node_topics_.get(),
-    topic_name,
-    std::move(cb),
-    rmw_qos_profile_default,
-    group,
-    false,
-    false,
-    msg_mem_strat,
-    allocator);
+  rclcpp::Parameter use_sim_time_param;
+  if (node_parameters_->get_parameter("use_sim_time", use_sim_time_param)) {
+    if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
+      if (use_sim_time_param.get_value<bool>() == true) {
+        parameter_state_ = SET_TRUE;
+        enable_ros_time();
+        create_clock_sub();
+      }
+    } else {
+      RCLCPP_ERROR(logger_, "Invalid type for parameter 'use_sim_time' %s should be bool",
+        use_sim_time_param.get_type_name().c_str());
+    }
+  } else {
+    RCLCPP_DEBUG(logger_, "'use_sim_time' parameter not set, using wall time by default.");
+  }
 
+  // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609
   parameter_client_ = std::make_shared<rclcpp::AsyncParametersClient>(
     node_base_,
     node_topics_,
     node_graph_,
     node_services_
-    );
+  );
   parameter_subscription_ =
     parameter_client_->on_parameter_event(std::bind(&TimeSource::on_parameter_event,
       this, std::placeholders::_1));
@@ -107,6 +115,9 @@ void TimeSource::detachNode()
   node_topics_.reset();
   node_graph_.reset();
   node_services_.reset();
+  node_logging_.reset();
+  node_clock_.reset();
+  node_parameters_.reset();
   disable_ros_time();
 }
 
@@ -118,11 +129,12 @@ void TimeSource::attachClock(std::shared_ptr<rclcpp::Clock> clock)
 
   std::lock_guard<std::mutex> guard(clock_list_lock_);
   associated_clocks_.push_back(clock);
-  // Set the clock if there's already data for it
+  // Set the clock to zero unless there's a recently received message
+  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
   if (last_msg_set_) {
-    auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
-    set_clock(time_msg, ros_time_active_, clock);
+    time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
   }
+  set_clock(time_msg, ros_time_active_, clock);
 }
 
 void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
@@ -132,13 +144,15 @@ void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> clock)
   if (result != associated_clocks_.end()) {
     associated_clocks_.erase(result);
   } else {
-    RCUTILS_LOG_ERROR("Failed to remove clock");
+    RCLCPP_ERROR(logger_, "failed to remove clock");
   }
 }
 
 TimeSource::~TimeSource()
 {
-  if (node_base_ || node_topics_ || node_graph_ || node_services_) {
+  if (node_base_ || node_topics_ || node_graph_ || node_services_ ||
+    node_logging_ || node_clock_ || node_parameters_)
+  {
     this->detachNode();
   }
 }
@@ -147,71 +161,72 @@ void TimeSource::set_clock(
   const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled,
   std::shared_ptr<rclcpp::Clock> clock)
 {
-  // Compute diff
-  rclcpp::Time msg_time = rclcpp::Time(*msg);
-  rclcpp::Time now = clock->now();
-  auto diff = now - msg_time;
-  rclcpp::TimeJump jump;
-  jump.delta_.nanoseconds = diff.nanoseconds();
-
-  // Compute jump type
-  if (clock->ros_time_is_active()) {
-    if (set_ros_time_enabled) {
-      jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE;
-    } else {
-      jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED;
-    }
-  } else if (!clock->ros_time_is_active()) {
-    if (set_ros_time_enabled) {
-      jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_ACTIVATED;
-    } else {
-      jump.jump_type_ = TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE;
-    }
-  }
-
-  if (jump.jump_type_ == TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE) {
-    // No change/no updates don't act.
-    return;
-  }
-
-  auto active_callbacks = clock->get_triggered_callback_handlers(jump);
-  clock->invoke_prejump_callbacks(active_callbacks);
-
   // Do change
-  if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED) {
+  if (!set_ros_time_enabled && clock->ros_time_is_active()) {
     disable_ros_time(clock);
-  } else if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED) {
+  } else if (set_ros_time_enabled && !clock->ros_time_is_active()) {
     enable_ros_time(clock);
   }
 
-  if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED ||
-    jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE)
-  {
-    auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), msg_time.nanoseconds());
-    if (ret != RCL_RET_OK) {
-      rclcpp::exceptions::throw_from_rcl_error(
-        ret, "Failed to set ros_time_override_status");
-    }
+  auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), rclcpp::Time(*msg).nanoseconds());
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "Failed to set ros_time_override_status");
   }
-  // Post change callbacks
-  clock->invoke_postjump_callbacks(active_callbacks, jump);
 }
 
 void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg)
 {
-  if (!this->ros_time_active_) {
+  if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) {
     enable_ros_time();
   }
   // Cache the last message in case a new clock is attached.
   last_msg_set_ = msg;
   auto time_msg = std::make_shared<builtin_interfaces::msg::Time>(msg->clock);
 
-  std::lock_guard<std::mutex> guard(clock_list_lock_);
-  for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
-    set_clock(time_msg, true, *it);
+  if (SET_TRUE == this->parameter_state_) {
+    std::lock_guard<std::mutex> guard(clock_list_lock_);
+    for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
+      set_clock(time_msg, true, *it);
+    }
   }
 }
 
+void TimeSource::create_clock_sub()
+{
+  std::lock_guard<std::mutex> guard(clock_sub_lock_);
+  if (clock_subscription_) {
+    // Subscription already created.
+    return;
+  }
+  const std::string topic_name = "/clock";
+
+  rclcpp::callback_group::CallbackGroup::SharedPtr group;
+  using rclcpp::message_memory_strategy::MessageMemoryStrategy;
+  auto msg_mem_strat = MessageMemoryStrategy<MessageT, Alloc>::create_default();
+  auto allocator = std::make_shared<Alloc>();
+  auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1);
+
+  clock_subscription_ = rclcpp::create_subscription<
+    MessageT, decltype(cb), Alloc, MessageT, SubscriptionT
+    >(
+    node_topics_.get(),
+    topic_name,
+    std::move(cb),
+    rmw_qos_profile_default,
+    group,
+    false,
+    false,
+    msg_mem_strat,
+    allocator);
+}
+
+void TimeSource::destroy_clock_sub()
+{
+  std::lock_guard<std::mutex> guard(clock_sub_lock_);
+  clock_subscription_.reset();
+}
+
 void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
 {
   // Filter for only 'use_sim_time' being added or changed.
@@ -220,15 +235,17 @@ void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::S
       rclcpp::ParameterEventsFilter::EventType::CHANGED});
   for (auto & it : filter.get_events()) {
     if (it.second->value.type != ParameterType::PARAMETER_BOOL) {
-      RCUTILS_LOG_ERROR("use_sim_time parameter set to something besides a bool");
+      RCLCPP_ERROR(logger_, "use_sim_time parameter set to something besides a bool");
       continue;
     }
     if (it.second->value.bool_value) {
       parameter_state_ = SET_TRUE;
       enable_ros_time();
+      create_clock_sub();
     } else {
       parameter_state_ = SET_FALSE;
       disable_ros_time();
+      destroy_clock_sub();
     }
   }
   // Handle the case that use_sim_time was deleted.
@@ -269,13 +286,14 @@ void TimeSource::enable_ros_time()
   // Local storage
   ros_time_active_ = true;
 
-  // Update all attached clocks
+  // Update all attached clocks to zero or last recorded time
   std::lock_guard<std::mutex> guard(clock_list_lock_);
+  auto time_msg = std::make_shared<builtin_interfaces::msg::Time>();
+  if (last_msg_set_) {
+    time_msg = std::make_shared<builtin_interfaces::msg::Time>(last_msg_set_->clock);
+  }
   for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) {
-    auto msg = std::make_shared<builtin_interfaces::msg::Time>();
-    msg->sec = 0;
-    msg->nanosec = 0;
-    set_clock(msg, true, *it);
+    set_clock(time_msg, true, *it);
   }
 }
 

rclcpp/src/rclcpp/timer.cpp

@@ -18,33 +18,48 @@
 #include <string>
 #include <memory>
 
+#include "rclcpp/contexts/default_context.hpp"
 #include "rcutils/logging_macros.h"
 
 using rclcpp::TimerBase;
 
-TimerBase::TimerBase(std::chrono::nanoseconds period)
+TimerBase::TimerBase(
+  rclcpp::Clock::SharedPtr clock,
+  std::chrono::nanoseconds period,
+  rclcpp::Context::SharedPtr context)
+: clock_(clock), timer_handle_(nullptr)
 {
+  if (nullptr == context) {
+    context = rclcpp::contexts::default_context::get_global_default_context();
+  }
+
+  auto rcl_context = context->get_rcl_context();
+
   timer_handle_ = std::shared_ptr<rcl_timer_t>(
-    new rcl_timer_t, [ = ](rcl_timer_t * timer)
+    new rcl_timer_t, [ = ](rcl_timer_t * timer) mutable
     {
       if (rcl_timer_fini(timer) != RCL_RET_OK) {
         RCUTILS_LOG_ERROR_NAMED(
           "rclcpp",
-          "Failed to clean up rcl timer handle: %s", rcl_get_error_string_safe());
+          "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str);
         rcl_reset_error();
       }
       delete timer;
+      // Captured shared pointers by copy, reset to make sure timer is finalized before clock
+      clock.reset();
+      rcl_context.reset();
     });
 
   *timer_handle_.get() = rcl_get_zero_initialized_timer();
 
+  rcl_clock_t * clock_handle = clock_->get_clock_handle();
   if (rcl_timer_init(
-      timer_handle_.get(), period.count(), nullptr,
+      timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr,
       rcl_get_default_allocator()) != RCL_RET_OK)
   {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
-      "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe());
+      "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str);
     rcl_reset_error();
   }
 }
@@ -56,7 +71,7 @@ void
 TimerBase::cancel()
 {
   if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string_safe());
+    throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string().str);
   }
 }
 
@@ -64,7 +79,7 @@ void
 TimerBase::reset()
 {
   if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string_safe());
+    throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string().str);
   }
 }
 
@@ -73,7 +88,7 @@ TimerBase::is_ready()
 {
   bool ready = false;
   if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) {
-    throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string_safe());
+    throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string().str);
   }
   return ready;
 }
@@ -87,7 +102,7 @@ TimerBase::time_until_trigger()
   {
     throw std::runtime_error(
             std::string("Timer could not get time until next call: ") +
-            rcl_get_error_string_safe());
+            rcl_get_error_string().str);
   }
   return std::chrono::nanoseconds(time_until_next_call);
 }

rclcpp/src/rclcpp/utilities.cpp

@@ -14,189 +14,50 @@
 
 #include "rclcpp/utilities.hpp"
 
-#include <atomic>
-#include <condition_variable>
-#include <csignal>
-#include <cstdio>
-#include <cstring>
-#include <map>
-#include <mutex>
 #include <string>
 #include <vector>
 
+#include "./signal_handler.hpp"
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/exceptions.hpp"
 
 #include "rcl/error_handling.h"
 #include "rcl/rcl.h"
 
-#include "rmw/error_handling.h"
-#include "rmw/rmw.h"
-
-#include "rcutils/logging_macros.h"
-
-// Determine if sigaction is available
-#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE
-#define HAS_SIGACTION
-#endif
-
-/// Represent the status of the global interrupt signal.
-static volatile sig_atomic_t g_signal_status = 0;
-/// Guard conditions for interrupting the rmw implementation when the global interrupt signal fired.
-static std::map<rcl_wait_set_t *, rcl_guard_condition_t> g_sigint_guard_cond_handles;
-/// Mutex to protect g_sigint_guard_cond_handles
-static std::mutex g_sigint_guard_cond_handles_mutex;
-/// Condition variable for timed sleep (see sleep_for).
-static std::condition_variable g_interrupt_condition_variable;
-static std::atomic<bool> g_is_interrupted(false);
-/// Mutex for protecting the global condition variable.
-static std::mutex g_interrupt_mutex;
-
-#ifdef HAS_SIGACTION
-static struct sigaction old_action;
-#else
-typedef void (* signal_handler_t)(int);
-static signal_handler_t old_signal_handler = 0;
-#endif
-
-#ifdef HAS_SIGACTION
-struct sigaction
-set_sigaction(int signal_value, const struct sigaction & action)
-#else
-signal_handler_t
-set_signal_handler(int signal_value, signal_handler_t signal_handler)
-#endif
+void
+rclcpp::init(int argc, char const * const argv[], const rclcpp::InitOptions & init_options)
 {
-#ifdef HAS_SIGACTION
-  struct sigaction old_action;
-  ssize_t ret = sigaction(signal_value, &action, &old_action);
-  if (ret == -1)
-#else
-  signal_handler_t old_signal_handler = std::signal(signal_value, signal_handler);
-  // NOLINTNEXTLINE(readability/braces)
-  if (old_signal_handler == SIG_ERR)
-#endif
-  {
-    const size_t error_length = 1024;
-    // NOLINTNEXTLINE(runtime/arrays)
-    char error_string[error_length];
-#ifndef _WIN32
-#if (defined(_GNU_SOURCE) && !defined(ANDROID))
-    char * msg = strerror_r(errno, error_string, error_length);
-    if (msg != error_string) {
-      strncpy(error_string, msg, error_length);
-      msg[error_length - 1] = '\0';
-    }
-#else
-    int error_status = strerror_r(errno, error_string, error_length);
-    if (error_status != 0) {
-      throw std::runtime_error("Failed to get error string for errno: " + std::to_string(errno));
-    }
-#endif
-#else
-    strerror_s(error_string, error_length, errno);
-#endif
-    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-    throw std::runtime_error(
-      std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") +
-      error_string);
-    // *INDENT-ON*
-  }
-
-#ifdef HAS_SIGACTION
-  return old_action;
-#else
-  return old_signal_handler;
-#endif
+  using rclcpp::contexts::default_context::get_global_default_context;
+  get_global_default_context()->init(argc, argv, init_options);
+  // Install the signal handlers.
+  rclcpp::install_signal_handlers();
 }
 
-void
-trigger_interrupt_guard_condition(int signal_value)
+bool
+rclcpp::install_signal_handlers()
 {
-  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) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "failed to trigger guard condition: %s", rcl_get_error_string_safe());
-      }
-    }
-  }
-  g_is_interrupted.store(true);
-  g_interrupt_condition_variable.notify_all();
+  return rclcpp::SignalHandler::get_global_signal_handler().install();
 }
 
-void
-#ifdef HAS_SIGACTION
-signal_handler(int signal_value, siginfo_t * siginfo, void * context)
-#else
-signal_handler(int signal_value)
-#endif
+bool
+rclcpp::signal_handlers_installed()
 {
-  // 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 {
-    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
-    {
-      old_action.sa_handler(signal_value);
-    }
-  }
-#else
-  if (old_signal_handler) {
-    old_signal_handler(signal_value);
-  }
-#endif
-
-  trigger_interrupt_guard_condition(signal_value);
+  return rclcpp::SignalHandler::get_global_signal_handler().is_installed();
 }
 
-void
-rclcpp::init(int argc, char const * const argv[])
+bool
+rclcpp::uninstall_signal_handlers()
 {
-  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::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::on_shutdown(
-    []() {
-      set_signal_handler(SIGINT, ::old_signal_handler);
-    });
-#endif
+  return rclcpp::SignalHandler::get_global_signal_handler().uninstall();
 }
 
 std::vector<std::string>
-rclcpp::init_and_remove_ros_arguments(int argc, char const * const argv[])
+rclcpp::init_and_remove_ros_arguments(
+  int argc,
+  char const * const argv[],
+  const rclcpp::InitOptions & init_options)
 {
-  rclcpp::init(argc, argv);
+  rclcpp::init(argc, argv, init_options);
   return rclcpp::remove_ros_arguments(argc, argv);
 }
 
@@ -210,7 +71,7 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
 
   ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
   if (RCL_RET_OK != ret) {
-    exceptions::throw_from_rcl_error(ret, "failed to parse arguments");
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to parse arguments");
   }
 
   int nonros_argc = 0;
@@ -225,7 +86,7 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
 
   if (RCL_RET_OK != ret) {
     // Not using throw_from_rcl_error, because we may need to append deallocation failures.
-    exceptions::RCLErrorBase base_exc(ret, rcl_get_error_state());
+    rclcpp::exceptions::RCLErrorBase base_exc(ret, rcl_get_error_state());
     rcl_reset_error();
     if (NULL != nonros_argv) {
       alloc.deallocate(nonros_argv, alloc.state);
@@ -233,10 +94,10 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
     if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) {
       base_exc.formatted_message += std::string(
         ", failed also to cleanup parsed arguments, leaking memory: ") +
-        rcl_get_error_string_safe();
+        rcl_get_error_string().str;
       rcl_reset_error();
     }
-    throw exceptions::RCLError(base_exc, "");
+    throw rclcpp::exceptions::RCLError(base_exc, "");
   }
 
   std::vector<std::string> return_arguments;
@@ -252,98 +113,72 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
 
   ret = rcl_arguments_fini(&parsed_args);
   if (RCL_RET_OK != ret) {
-    exceptions::throw_from_rcl_error(ret, "failed to cleanup parsed arguments, leaking memory");
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "failed to cleanup parsed arguments, leaking memory");
   }
 
   return return_arguments;
 }
 
 bool
-rclcpp::ok()
+rclcpp::ok(rclcpp::Context::SharedPtr context)
 {
-  return ::g_signal_status == 0;
-}
-
-static std::mutex on_shutdown_mutex_;
-static std::vector<std::function<void(void)>> on_shutdown_callbacks_;
-
-void
-rclcpp::shutdown()
-{
-  trigger_interrupt_guard_condition(SIGINT);
-
-  {
-    std::lock_guard<std::mutex> lock(on_shutdown_mutex_);
-    for (auto & on_shutdown_callback : on_shutdown_callbacks_) {
-      on_shutdown_callback();
-    }
-  }
-}
-
-void
-rclcpp::on_shutdown(std::function<void(void)> callback)
-{
-  std::lock_guard<std::mutex> lock(on_shutdown_mutex_);
-  on_shutdown_callbacks_.push_back(callback);
-}
-
-rcl_guard_condition_t *
-rclcpp::get_sigint_guard_condition(rcl_wait_set_t * wait_set)
-{
-  std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
-  auto kv = g_sigint_guard_cond_handles.find(wait_set);
-  if (kv != g_sigint_guard_cond_handles.end()) {
-    return &kv->second;
-  } else {
-    rcl_guard_condition_t handle =
-      rcl_get_zero_initialized_guard_condition();
-    rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options();
-    if (rcl_guard_condition_init(&handle, options) != RCL_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(std::string(
-        "Couldn't initialize guard condition: ") + rcl_get_error_string_safe());
-      // *INDENT-ON*
-    }
-    g_sigint_guard_cond_handles[wait_set] = handle;
-    return &g_sigint_guard_cond_handles[wait_set];
-  }
-}
-
-void
-rclcpp::release_sigint_guard_condition(rcl_wait_set_t * wait_set)
-{
-  std::lock_guard<std::mutex> lock(g_sigint_guard_cond_handles_mutex);
-  auto kv = g_sigint_guard_cond_handles.find(wait_set);
-  if (kv != g_sigint_guard_cond_handles.end()) {
-    if (rcl_guard_condition_fini(&kv->second) != RCL_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-      throw std::runtime_error(std::string(
-        "Failed to destroy sigint guard condition: ") +
-        rcl_get_error_string_safe());
-      // *INDENT-ON*
-    }
-    g_sigint_guard_cond_handles.erase(kv);
-  } else {
-    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
-    throw std::runtime_error(std::string(
-      "Tried to release sigint guard condition for nonexistent wait set"));
-    // *INDENT-ON*
+  using rclcpp::contexts::default_context::get_global_default_context;
+  if (nullptr == context) {
+    context = get_global_default_context();
   }
+  return context->is_valid();
 }
 
 bool
-rclcpp::sleep_for(const std::chrono::nanoseconds & nanoseconds)
+rclcpp::is_initialized(rclcpp::Context::SharedPtr context)
 {
-  std::chrono::nanoseconds time_left = nanoseconds;
-  {
-    std::unique_lock<std::mutex> lock(::g_interrupt_mutex);
-    auto start = std::chrono::steady_clock::now();
-    ::g_interrupt_condition_variable.wait_for(lock, nanoseconds);
-    time_left -= std::chrono::steady_clock::now() - start;
-  }
-  if (time_left > std::chrono::nanoseconds::zero() && !g_is_interrupted) {
-    return sleep_for(time_left);
-  }
-  // Return true if the timeout elapsed successfully, otherwise false.
-  return !g_is_interrupted;
+  return rclcpp::ok(context);
+}
+
+bool
+rclcpp::shutdown(rclcpp::Context::SharedPtr context, const std::string & reason)
+{
+  using rclcpp::contexts::default_context::get_global_default_context;
+  auto default_context = get_global_default_context();
+  if (nullptr == context) {
+    context = default_context;
+  }
+  bool ret = context->shutdown(reason);
+  if (context == default_context) {
+    rclcpp::uninstall_signal_handlers();
+  }
+  return ret;
+}
+
+void
+rclcpp::on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr context)
+{
+  using rclcpp::contexts::default_context::get_global_default_context;
+  if (nullptr == context) {
+    context = get_global_default_context();
+  }
+  context->on_shutdown(callback);
+}
+
+bool
+rclcpp::sleep_for(const std::chrono::nanoseconds & nanoseconds, rclcpp::Context::SharedPtr context)
+{
+  using rclcpp::contexts::default_context::get_global_default_context;
+  if (nullptr == context) {
+    context = get_global_default_context();
+  }
+  return context->sleep_for(nanoseconds);
+}
+
+const char *
+rclcpp::get_c_string(const char * string_in)
+{
+  return string_in;
+}
+
+const char *
+rclcpp::get_c_string(const std::string & string_in)
+{
+  return string_in.c_str();
 }

rclcpp/src/rclcpp/waitable.cpp

@@ -0,0 +1,47 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/waitable.hpp"
+
+using rclcpp::Waitable;
+
+size_t
+Waitable::get_number_of_ready_subscriptions()
+{
+  return 0u;
+}
+
+size_t
+Waitable::get_number_of_ready_timers()
+{
+  return 0u;
+}
+
+size_t
+Waitable::get_number_of_ready_clients()
+{
+  return 0u;
+}
+
+size_t
+Waitable::get_number_of_ready_services()
+{
+  return 0u;
+}
+
+size_t
+Waitable::get_number_of_ready_guard_conditions()
+{
+  return 0u;
+}

rclcpp/test/test_duration.cpp

@@ -97,3 +97,33 @@ TEST(TestDuration, overflows) {
   EXPECT_THROW(base_d_neg * (-4), std::overflow_error);
   EXPECT_THROW(base_d_neg * 4, std::underflow_error);
 }
+
+TEST(TestDuration, negative_duration) {
+  rclcpp::Duration assignable_duration = rclcpp::Duration(0) - rclcpp::Duration(5, 0);
+
+  {
+    // avoid windows converting a literal number less than -INT_MAX to unsigned int C4146
+    int64_t expected_value = -5000;
+    expected_value *= 1000 * 1000;
+    EXPECT_EQ(expected_value, assignable_duration.nanoseconds());
+  }
+
+  {
+    builtin_interfaces::msg::Duration duration_msg;
+    duration_msg.sec = -4;
+    duration_msg.nanosec = 250000000;
+
+    assignable_duration = duration_msg;
+    // avoid windows converting a literal number less than -INT_MAX to unsigned int C4146
+    int64_t expected_value = -3750;
+    expected_value *= 1000 * 1000;
+    EXPECT_EQ(expected_value, assignable_duration.nanoseconds());
+  }
+}
+
+TEST(TestDuration, maximum_duration) {
+  rclcpp::Duration max_duration = rclcpp::Duration::max();
+  rclcpp::Duration max(std::numeric_limits<int32_t>::max(), 999999999);
+
+  EXPECT_EQ(max_duration, max);
+}

rclcpp/test/test_executor.cpp

@@ -28,7 +28,7 @@
 
 using namespace std::chrono_literals;
 
-class TestExcutors : public ::testing::Test
+class TestExecutors : public ::testing::Test
 {
 protected:
   static void SetUpTestCase()
@@ -50,7 +50,7 @@ protected:
 };
 
 // Make sure that executors detach from nodes when destructing
-TEST_F(TestExcutors, detachOnDestruction) {
+TEST_F(TestExecutors, detachOnDestruction) {
   {
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(node);

rclcpp/test/test_init.cpp

@@ -0,0 +1,29 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include "rclcpp/utilities.hpp"
+
+TEST(TestInit, is_initialized) {
+  EXPECT_FALSE(rclcpp::is_initialized());
+
+  rclcpp::init(0, nullptr);
+
+  EXPECT_TRUE(rclcpp::is_initialized());
+
+  rclcpp::shutdown();
+
+  EXPECT_FALSE(rclcpp::is_initialized());
+}

rclcpp/test/test_logging.cpp

@@ -92,6 +92,26 @@ TEST_F(TestLoggingMacros, test_logging_named) {
   EXPECT_EQ("message 3", g_last_log_event.message);
 }
 
+TEST_F(TestLoggingMacros, test_logging_string) {
+  for (std::string i : {"one", "two", "three"}) {
+    RCLCPP_DEBUG(g_logger, "message " + i);
+  }
+  EXPECT_EQ(3u, g_log_calls);
+  EXPECT_EQ("message three", g_last_log_event.message);
+
+  RCLCPP_DEBUG(g_logger, "message " "four");
+  EXPECT_EQ("message four", g_last_log_event.message);
+
+  RCLCPP_DEBUG(g_logger, std::string("message " "five"));
+  EXPECT_EQ("message five", g_last_log_event.message);
+
+  RCLCPP_DEBUG(g_logger, std::string("message %s"), "six");
+  EXPECT_EQ("message six", g_last_log_event.message);
+
+  RCLCPP_DEBUG(g_logger, "message seven");
+  EXPECT_EQ("message seven", g_last_log_event.message);
+}
+
 TEST_F(TestLoggingMacros, test_logging_once) {
   for (int i : {1, 2, 3}) {
     RCLCPP_INFO_ONCE(g_logger, "message %d", i);

rclcpp/test/test_node.cpp

@@ -111,5 +111,5 @@ TEST_F(TestNode, now) {
   auto now_builtin = node->now().nanoseconds();
   auto now_external = clock->now().nanoseconds();
   EXPECT_GE(now_external, now_builtin);
-  EXPECT_LT(now_external - now_builtin, 50000L);
+  EXPECT_LT(now_external - now_builtin, 5000000L);
 }

rclcpp/test/test_time.cpp

@@ -244,3 +244,9 @@ TEST(TestTime, overflows) {
   rclcpp::Time two_time(2);
   EXPECT_NO_THROW(one_time - two_time);
 }
+
+TEST(TestTime, seconds) {
+  EXPECT_DOUBLE_EQ(0.0, rclcpp::Time(0, 0).seconds());
+  EXPECT_DOUBLE_EQ(4.5, rclcpp::Time(4, 500000000).seconds());
+  EXPECT_DOUBLE_EQ(2.5, rclcpp::Time(0, 2500000000).seconds());
+}

rclcpp/test/test_time_source.cpp

@@ -37,6 +37,11 @@ protected:
     rclcpp::init(0, nullptr);
   }
 
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+
   void SetUp()
   {
     node = std::make_shared<rclcpp::Node>("my_node");
@@ -50,179 +55,6 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
-TEST_F(TestTimeSource, detachUnattached) {
-  rclcpp::TimeSource ts;
-
-  ASSERT_NO_THROW(ts.detachNode());
-
-  // Try multiple detach to see if error
-  ASSERT_NO_THROW(ts.detachNode());
-}
-
-TEST_F(TestTimeSource, reattach) {
-  rclcpp::TimeSource ts;
-  // Try reattach
-  ASSERT_NO_THROW(ts.attachNode(node));
-  ASSERT_NO_THROW(ts.attachNode(node));
-}
-
-TEST_F(TestTimeSource, ROS_time_valid) {
-  rclcpp::TimeSource ts;
-  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
-
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
-  ts.attachClock(ros_clock);
-  auto now = ros_clock->now();
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
-
-  ts.attachNode(node);
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
-
-  ts.detachNode();
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
-
-  ts.attachNode(node);
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
-}
-
-TEST_F(TestTimeSource, clock) {
-  rclcpp::TimeSource ts(node);
-  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
-  ts.attachClock(ros_clock);
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
-
-  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
-      rmw_qos_profile_default);
-  rclcpp::WallRate loop_rate(50);
-  for (int i = 0; i < 5; ++i) {
-    if (!rclcpp::ok()) {
-      break;  // Break for ctrl-c
-    }
-    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
-    msg->clock.sec = i;
-    msg->clock.nanosec = 1000;
-    clock_pub->publish(msg);
-    rclcpp::spin_some(node);
-    loop_rate.sleep();
-  }
-  auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
-  auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
-
-  // Now that we've recieved a message it should be active with parameter unset
-  EXPECT_TRUE(ros_clock->ros_time_is_active());
-
-  auto t_out = ros_clock->now();
-
-  EXPECT_NE(0L, t_out.nanoseconds());
-  EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
-  EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
-}
-
-class CallbackObject
-{
-public:
-  CallbackObject()
-  : last_precallback_id_(0),
-    last_postcallback_id_(0)
-  {}
-  int last_precallback_id_;
-  void pre_callback(int id) {last_precallback_id_ = id;}
-
-  int last_postcallback_id_;
-  rclcpp::TimeJump last_timejump_;
-  void post_callback(const rclcpp::TimeJump & jump, int id)
-  {
-    last_postcallback_id_ = id; last_timejump_ = jump;
-  }
-};
-
-TEST_F(TestTimeSource, callbacks) {
-  CallbackObject cbo;
-  rclcpp::JumpThreshold jump_threshold;
-  jump_threshold.min_forward_ = 0;
-  jump_threshold.min_backward_ = 0;
-  jump_threshold.on_clock_change_ = true;
-
-  rclcpp::TimeSource ts(node);
-  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
-
-  // Register a callback for time jumps
-  rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback(
-    std::bind(&CallbackObject::pre_callback, &cbo, 1),
-    std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1),
-    jump_threshold);
-
-  EXPECT_EQ(0, cbo.last_precallback_id_);
-  EXPECT_EQ(0, cbo.last_postcallback_id_);
-
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
-
-  ts.attachClock(ros_clock);
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
-
-  auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
-      rmw_qos_profile_default);
-
-  rclcpp::WallRate loop_rate(50);
-  for (int i = 0; i < 5; ++i) {
-    if (!rclcpp::ok()) {
-      break;  // Break for ctrl-c
-    }
-    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
-    msg->clock.sec = i;
-    msg->clock.nanosec = 1000;
-    clock_pub->publish(msg);
-    rclcpp::spin_some(node);
-    loop_rate.sleep();
-  }
-  auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
-  auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
-
-  EXPECT_EQ(1, cbo.last_precallback_id_);
-  EXPECT_EQ(1, cbo.last_postcallback_id_);
-
-  // Now that we've recieved a message it should be active with parameter unset
-  EXPECT_TRUE(ros_clock->ros_time_is_active());
-
-  auto t_out = ros_clock->now();
-
-  EXPECT_NE(0L, t_out.nanoseconds());
-  EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
-  EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
-
-
-  // Change callbacks
-  rclcpp::JumpHandler::SharedPtr callback_handler2 = ros_clock->create_jump_callback(
-    std::bind(&CallbackObject::pre_callback, &cbo, 2),
-    std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2),
-    jump_threshold);
-
-  for (int i = 0; i < 5; ++i) {
-    if (!rclcpp::ok()) {
-      break;  // Break for ctrl-c
-    }
-    auto msg = std::make_shared<rosgraph_msgs::msg::Clock>();
-    msg->clock.sec = i;
-    msg->clock.nanosec = 2000;
-    clock_pub->publish(msg);
-    rclcpp::spin_some(node);
-    loop_rate.sleep();
-  }
-
-  EXPECT_EQ(2, cbo.last_precallback_id_);
-  EXPECT_EQ(2, cbo.last_postcallback_id_);
-
-  // Now that we've recieved a message it should be active with parameter unset
-  EXPECT_TRUE(ros_clock->ros_time_is_active());
-
-  t_out = ros_clock->now();
-
-  EXPECT_NE(0L, t_out.nanoseconds());
-  EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
-  EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
-}
-
 void trigger_clock_changes(
   rclcpp::Node::SharedPtr node)
 {
@@ -246,13 +78,235 @@ void trigger_clock_changes(
   }
 }
 
+void set_use_sim_time_parameter(rclcpp::Node::SharedPtr node, rclcpp::ParameterValue value)
+{
+  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
+
+  using namespace std::chrono_literals;
+  EXPECT_TRUE(parameters_client->wait_for_service(2s));
+  auto set_parameters_results = parameters_client->set_parameters({
+    rclcpp::Parameter("use_sim_time", value)
+  });
+  for (auto & result : set_parameters_results) {
+    EXPECT_TRUE(result.successful);
+  }
+  // SyncParametersClient returns when parameters have been set on the node_parameters interface,
+  // but it doesn't mean the on_parameter_event subscription in TimeSource has been called.
+  // Spin some to handle that subscription.
+  rclcpp::spin_some(node);
+}
+
+TEST_F(TestTimeSource, detachUnattached) {
+  rclcpp::TimeSource ts;
+
+  ASSERT_NO_THROW(ts.detachNode());
+
+  // Try multiple detach to see if error
+  ASSERT_NO_THROW(ts.detachNode());
+}
+
+TEST_F(TestTimeSource, reattach) {
+  rclcpp::TimeSource ts;
+  // Try reattach
+  ASSERT_NO_THROW(ts.attachNode(node));
+  ASSERT_NO_THROW(ts.attachNode(node));
+}
+
+TEST_F(TestTimeSource, ROS_time_valid_attach_detach) {
+  rclcpp::TimeSource ts;
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+  ts.attachClock(ros_clock);
+  auto now = ros_clock->now();
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachNode(node);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.detachNode();
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachNode(node);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+}
+
+TEST_F(TestTimeSource, ROS_time_valid_wall_time) {
+  rclcpp::TimeSource ts;
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  auto ros_clock2 = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  ts.attachClock(ros_clock);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachNode(node);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachClock(ros_clock2);
+  EXPECT_FALSE(ros_clock2->ros_time_is_active());
+}
+
+TEST_F(TestTimeSource, ROS_time_valid_sim_time) {
+  rclcpp::TimeSource ts;
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  auto ros_clock2 = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  ts.attachClock(ros_clock);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  node->set_parameter_if_not_set("use_sim_time", true);
+  ts.attachNode(node);
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  ts.attachClock(ros_clock2);
+  EXPECT_TRUE(ros_clock2->ros_time_is_active());
+}
+
+TEST_F(TestTimeSource, clock) {
+  rclcpp::TimeSource ts(node);
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+  ts.attachClock(ros_clock);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  trigger_clock_changes(node);
+
+  // Even now that we've recieved a message, ROS time should still not be active since the
+  // parameter has not been explicitly set.
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  // Activate ROS time.
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  trigger_clock_changes(node);
+
+  auto t_out = ros_clock->now();
+
+  // Time from clock should now reflect what was published on the /clock topic.
+  auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
+  auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
+  EXPECT_NE(0L, t_out.nanoseconds());
+  EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
+  EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
+}
+
+class CallbackObject
+{
+public:
+  int pre_callback_calls_ = 0;
+  int last_precallback_id_ = 0;
+  void pre_callback(int id)
+  {
+    last_precallback_id_ = id;
+    ++pre_callback_calls_;
+  }
+
+  int post_callback_calls_ = 0;
+  int last_postcallback_id_ = 0;
+  rcl_time_jump_t last_timejump_;
+  void post_callback(const rcl_time_jump_t & jump, int id)
+  {
+    last_postcallback_id_ = id; last_timejump_ = jump;
+    ++post_callback_calls_;
+  }
+};
+
+TEST_F(TestTimeSource, callbacks) {
+  CallbackObject cbo;
+  rcl_jump_threshold_t jump_threshold;
+  jump_threshold.min_forward.nanoseconds = 0;
+  jump_threshold.min_backward.nanoseconds = 0;
+  jump_threshold.on_clock_change = true;
+
+  rclcpp::TimeSource ts(node);
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  // Register a callback for time jumps
+  rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback(
+    std::bind(&CallbackObject::pre_callback, &cbo, 1),
+    std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1),
+    jump_threshold);
+
+  EXPECT_EQ(0, cbo.last_precallback_id_);
+  EXPECT_EQ(0, cbo.last_postcallback_id_);
+
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  ts.attachClock(ros_clock);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  trigger_clock_changes(node);
+  auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
+  auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
+
+  // Callbacks will not be triggered since ROS time is not active.
+  EXPECT_EQ(0, cbo.last_precallback_id_);
+  EXPECT_EQ(0, cbo.last_postcallback_id_);
+
+  // Activate ROS time.
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  trigger_clock_changes(node);
+
+  auto t_out = ros_clock->now();
+
+  EXPECT_NE(0L, t_out.nanoseconds());
+  EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
+  EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
+
+  // Callbacks will now have been triggered since ROS time is active.
+  EXPECT_EQ(1, cbo.last_precallback_id_);
+  EXPECT_EQ(1, cbo.last_postcallback_id_);
+
+  // Change callbacks
+  rclcpp::JumpHandler::SharedPtr callback_handler2 = ros_clock->create_jump_callback(
+    std::bind(&CallbackObject::pre_callback, &cbo, 2),
+    std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2),
+    jump_threshold);
+
+  trigger_clock_changes(node);
+
+  EXPECT_EQ(2, cbo.last_precallback_id_);
+  EXPECT_EQ(2, cbo.last_postcallback_id_);
+
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+
+  t_out = ros_clock->now();
+
+  EXPECT_NE(0L, t_out.nanoseconds());
+  EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds());
+  EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
+
+  // Register a callback handler with only pre_callback
+  rclcpp::JumpHandler::SharedPtr callback_handler3 = ros_clock->create_jump_callback(
+    std::bind(&CallbackObject::pre_callback, &cbo, 3),
+    std::function<void(rcl_time_jump_t)>(),
+    jump_threshold);
+
+  trigger_clock_changes(node);
+  EXPECT_EQ(3, cbo.last_precallback_id_);
+  EXPECT_EQ(2, cbo.last_postcallback_id_);
+
+  // Register a callback handler with only post_callback
+  rclcpp::JumpHandler::SharedPtr callback_handler4 = ros_clock->create_jump_callback(
+    std::function<void()>(),
+    std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 4),
+    jump_threshold);
+
+  trigger_clock_changes(node);
+  EXPECT_EQ(3, cbo.last_precallback_id_);
+  EXPECT_EQ(4, cbo.last_postcallback_id_);
+}
+
 
 TEST_F(TestTimeSource, callback_handler_erasure) {
   CallbackObject cbo;
-  rclcpp::JumpThreshold jump_threshold;
-  jump_threshold.min_forward_ = 0;
-  jump_threshold.min_backward_ = 0;
-  jump_threshold.on_clock_change_ = true;
+  rcl_jump_threshold_t jump_threshold;
+  jump_threshold.min_forward.nanoseconds = 0;
+  jump_threshold.min_backward.nanoseconds = 0;
+  jump_threshold.on_clock_change = true;
 
   rclcpp::TimeSource ts(node);
   auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
@@ -271,24 +325,23 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
     std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1),
     jump_threshold);
 
-
+  // Callbacks will not be triggered since ROS time is not active.
   EXPECT_EQ(0, cbo.last_precallback_id_);
   EXPECT_EQ(0, cbo.last_postcallback_id_);
 
-
-  EXPECT_FALSE(ros_clock->ros_time_is_active());
+  // Activate ROS time.
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
 
   trigger_clock_changes(node);
 
   auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
   auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
 
+  // Callbacks will now have been triggered since ROS time is active.
   EXPECT_EQ(1, cbo.last_precallback_id_);
   EXPECT_EQ(1, cbo.last_postcallback_id_);
 
-  // Now that we've recieved a message it should be active with parameter unset
-  EXPECT_TRUE(ros_clock->ros_time_is_active());
-
   auto t_out = ros_clock->now();
 
   EXPECT_NE(0L, t_out.nanoseconds());
@@ -306,11 +359,9 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
 
   trigger_clock_changes(node);
 
-
   EXPECT_EQ(2, cbo.last_precallback_id_);
   EXPECT_EQ(2, cbo.last_postcallback_id_);
 
-  // Now that we've recieved a message it should be active with parameter unset
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
   t_out = ros_clock->now();
@@ -329,46 +380,56 @@ TEST_F(TestTimeSource, parameter_activation) {
   ts.attachClock(ros_clock);
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
-
-  using namespace std::chrono_literals;
-  EXPECT_TRUE(parameters_client->wait_for_service(2s));
-  auto set_parameters_results = parameters_client->set_parameters({
-    rclcpp::Parameter("use_sim_time", true)
-  });
-  for (auto & result : set_parameters_results) {
-    EXPECT_TRUE(result.successful);
-  }
-  // SyncParametersClient returns when parameters have been set on the node_parameters interface,
-  // but it doesn't mean the on_parameter_event subscription in TimeSource has been called.
-  // Spin some to handle that subscription.
-  rclcpp::spin_some(node);
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
-  set_parameters_results = parameters_client->set_parameters({
-    rclcpp::Parameter("use_sim_time", rclcpp::ParameterType::PARAMETER_NOT_SET)
-  });
-  for (auto & result : set_parameters_results) {
-    EXPECT_TRUE(result.successful);
-  }
-  rclcpp::spin_some(node);
+  set_use_sim_time_parameter(
+    node, rclcpp::ParameterValue());
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
-  set_parameters_results = parameters_client->set_parameters({
-    rclcpp::Parameter("use_sim_time", false)
-  });
-  for (auto & result : set_parameters_results) {
-    EXPECT_TRUE(result.successful);
-  }
-  rclcpp::spin_some(node);
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(false));
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
-  set_parameters_results = parameters_client->set_parameters({
-    rclcpp::Parameter("use_sim_time", rclcpp::ParameterType::PARAMETER_NOT_SET)
-  });
-  for (auto & result : set_parameters_results) {
-    EXPECT_TRUE(result.successful);
-  }
-  rclcpp::spin_some(node);
+  set_use_sim_time_parameter(
+    node, rclcpp::ParameterValue());
   EXPECT_FALSE(ros_clock->ros_time_is_active());
+
+  // If the use_sim_time parameter is not explicitly set to True, this clock's use of sim time
+  // should not be affected by the presence of a clock publisher.
+  trigger_clock_changes(node);
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(false));
+  EXPECT_FALSE(ros_clock->ros_time_is_active());
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  EXPECT_TRUE(ros_clock->ros_time_is_active());
+}
+
+TEST_F(TestTimeSource, no_pre_jump_callback) {
+  CallbackObject cbo;
+  rcl_jump_threshold_t jump_threshold;
+  jump_threshold.min_forward.nanoseconds = 0;
+  jump_threshold.min_backward.nanoseconds = 0;
+  jump_threshold.on_clock_change = true;
+
+  rclcpp::TimeSource ts(node);
+  auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+
+  // Register a callback for time jumps
+  rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback(
+    nullptr,
+    std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1),
+    jump_threshold);
+
+  ASSERT_EQ(0, cbo.last_precallback_id_);
+  ASSERT_EQ(0, cbo.last_postcallback_id_);
+  ts.attachClock(ros_clock);
+
+  // Activate ROS time
+  set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
+  ASSERT_TRUE(ros_clock->ros_time_is_active());
+
+  EXPECT_EQ(0, cbo.last_precallback_id_);
+  EXPECT_EQ(0, cbo.pre_callback_calls_);
+  EXPECT_EQ(1, cbo.last_postcallback_id_);
+  EXPECT_EQ(1, cbo.post_callback_calls_);
 }

rclcpp/test/test_utilities.cpp

@@ -17,6 +17,7 @@
 #include <string>
 #include <memory>
 
+#include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/utilities.hpp"
 
@@ -48,4 +49,35 @@ TEST(TestUtilities, init_with_args) {
 
   ASSERT_EQ(1u, other_args.size());
   ASSERT_EQ(std::string{"process_name"}, other_args[0]);
+
+  EXPECT_TRUE(rclcpp::ok());
+  rclcpp::shutdown();
+}
+
+TEST(TestUtilities, multi_init) {
+  auto context1 = std::make_shared<rclcpp::contexts::default_context::DefaultContext>();
+  auto context2 = std::make_shared<rclcpp::contexts::default_context::DefaultContext>();
+
+  EXPECT_FALSE(rclcpp::ok(context1));
+  EXPECT_FALSE(rclcpp::ok(context2));
+
+  context1->init(0, nullptr);
+
+  EXPECT_TRUE(rclcpp::ok(context1));
+  EXPECT_FALSE(rclcpp::ok(context2));
+
+  context2->init(0, nullptr);
+
+  EXPECT_TRUE(rclcpp::ok(context1));
+  EXPECT_TRUE(rclcpp::ok(context2));
+
+  rclcpp::shutdown(context1);
+
+  EXPECT_FALSE(rclcpp::ok(context1));
+  EXPECT_TRUE(rclcpp::ok(context2));
+
+  rclcpp::shutdown(context2);
+
+  EXPECT_FALSE(rclcpp::ok(context1));
+  EXPECT_FALSE(rclcpp::ok(context2));
 }

rclcpp_action/CHANGELOG.rst

@@ -0,0 +1,15 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rclcpp_action
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.6.2 (2018-12-13)
+------------------
+
+0.6.1 (2018-12-07)
+------------------
+* Added wait_for_action_server() for action clients (`#598 <https://github.com/ros2/rclcpp/issues/598>`_)
+* Updated to adapt to action implicit changes (`#602 <https://github.com/ros2/rclcpp/issues/602>`_)
+* Added rclcpp_action Server implementation (`#593 <https://github.com/ros2/rclcpp/issues/593>`_)
+* Added action client implementation (`#594 <https://github.com/ros2/rclcpp/issues/594>`_)
+* Added skeleton for Action Server and Client (`#579 <https://github.com/ros2/rclcpp/issues/579>`_)
+* Contributors: Michel Hidalgo, Shane Loretz, William Woodall

rclcpp_action/CMakeLists.txt

@@ -0,0 +1,92 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(rclcpp_action)
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(action_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rcl_action REQUIRED)
+find_package(rosidl_generator_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")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+include_directories(include)
+
+set(${PROJECT_NAME}_SRCS
+  src/client.cpp
+  src/server.cpp
+  src/server_goal_handle.cpp
+  src/types.cpp
+)
+
+add_library(${PROJECT_NAME}
+  ${${PROJECT_NAME}_SRCS})
+
+ament_target_dependencies(${PROJECT_NAME}
+  "action_msgs"
+  "rcl_action"
+  "rclcpp"
+  "rosidl_generator_c"
+  "rosidl_generator_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}
+  PRIVATE "RCLCPP_ACTION_BUILDING_LIBRARY")
+
+install(
+  DIRECTORY include/
+  DESTINATION include)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# specific order: dependents before dependencies
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_export_dependencies(ament_cmake)
+ament_export_dependencies(action_msgs)
+ament_export_dependencies(rclcpp)
+ament_export_dependencies(rcl_action)
+ament_export_dependencies(rosidl_generator_c)
+ament_export_dependencies(rosidl_generator_cpp)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(test_client test/test_client.cpp)
+  if(TARGET test_client)
+    ament_target_dependencies(test_client
+      "test_msgs"
+    )
+    target_link_libraries(test_client
+      ${PROJECT_NAME}
+    )
+  endif()
+
+  ament_add_gtest(test_server test/test_server.cpp)
+  if(TARGET test_server)
+    ament_target_dependencies(test_server
+      "test_msgs"
+    )
+    target_link_libraries(test_server
+      ${PROJECT_NAME}
+    )
+  endif()
+
+endif()
+
+ament_package()

rclcpp_action/include/rclcpp_action/client.hpp

@@ -0,0 +1,544 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP_ACTION__CLIENT_HPP_
+#define RCLCPP_ACTION__CLIENT_HPP_
+
+#include <rclcpp/macros.hpp>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
+#include <rclcpp/node_interfaces/node_graph_interface.hpp>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/waitable.hpp>
+
+#include <rosidl_generator_c/action_type_support_struct.h>
+#include <rosidl_typesupport_cpp/action_type_support.hpp>
+
+#include <algorithm>
+#include <chrono>
+#include <functional>
+#include <future>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <utility>
+
+#include "rclcpp_action/client_goal_handle.hpp"
+#include "rclcpp_action/types.hpp"
+#include "rclcpp_action/visibility_control.hpp"
+
+
+namespace rclcpp_action
+{
+// Forward declaration
+class ClientBaseImpl;
+
+/// Base Action Client implementation
+/// \internal
+/**
+ * This class should not be used directly by users wanting to create an aciton client.
+ * Instead users should use `rclcpp_action::Client<>`.
+ *
+ * Internally, this class is responsible for interfacing with the `rcl_action` API.
+ */
+class ClientBase : public rclcpp::Waitable
+{
+public:
+  RCLCPP_ACTION_PUBLIC
+  virtual ~ClientBase();
+
+  /// Return true if there is an action server that is ready to take goal requests.
+  RCLCPP_ACTION_PUBLIC
+  bool
+  action_server_is_ready() const;
+
+  /// Wait for action_server_is_ready() to become true, or until the given timeout is reached.
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_action_server(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+  {
+    return wait_for_action_server_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
+  // -------------
+  // Waitables API
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_subscriptions() override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_timers() override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_clients() override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_services() override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_guard_conditions() override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute() override;
+
+  // End Waitables API
+  // -----------------
+
+protected:
+  RCLCPP_ACTION_PUBLIC
+  ClientBase(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    const std::string & action_name,
+    const rosidl_action_type_support_t * type_support,
+    const rcl_action_client_options_t & options);
+
+  /// Wait for action_server_is_ready() to become true, or until the given timeout is reached.
+  RCLCPP_ACTION_PUBLIC
+  bool
+  wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout);
+
+  // -----------------------------------------------------
+  // API for communication between ClientBase and Client<>
+  using ResponseCallback = std::function<void (std::shared_ptr<void> response)>;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  rclcpp::Logger get_logger();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  GoalID
+  generate_goal_id();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  send_goal_request(
+    std::shared_ptr<void> request,
+    ResponseCallback callback);
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  send_result_request(
+    std::shared_ptr<void> request,
+    ResponseCallback callback);
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  send_cancel_request(
+    std::shared_ptr<void> request,
+    ResponseCallback callback);
+
+  /// \internal
+  virtual
+  std::shared_ptr<void>
+  create_goal_response() const = 0;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  handle_goal_response(
+    const rmw_request_id_t & response_header,
+    std::shared_ptr<void> goal_response);
+
+  /// \internal
+  virtual
+  std::shared_ptr<void>
+  create_result_response() const = 0;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  handle_result_response(
+    const rmw_request_id_t & response_header,
+    std::shared_ptr<void> result_response);
+
+  /// \internal
+  virtual
+  std::shared_ptr<void>
+  create_cancel_response() const = 0;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  handle_cancel_response(
+    const rmw_request_id_t & response_header,
+    std::shared_ptr<void> cancel_response);
+
+  /// \internal
+  virtual
+  std::shared_ptr<void>
+  create_feedback_message() const = 0;
+
+  /// \internal
+  virtual
+  void
+  handle_feedback_message(std::shared_ptr<void> message) = 0;
+
+  /// \internal
+  virtual
+  std::shared_ptr<void>
+  create_status_message() const = 0;
+
+  /// \internal
+  virtual
+  void
+  handle_status_message(std::shared_ptr<void> message) = 0;
+
+  // End API for communication between ClientBase and Client<>
+  // ---------------------------------------------------------
+
+private:
+  std::unique_ptr<ClientBaseImpl> pimpl_;
+};
+
+/// Action Client
+/**
+ * This class creates an action client.
+ *
+ * Create an instance of this server using `rclcpp_action::create_client()`.
+ *
+ * Internally, this class is responsible for:
+ *  - coverting between the C++ action type and generic types for `rclcpp_action::ClientBase`, and
+ *  - calling user callbacks.
+ */
+template<typename ActionT>
+class Client : public ClientBase
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client<ActionT>)
+
+  using Goal = typename ActionT::Goal;
+  using Feedback = typename ActionT::Feedback;
+  using GoalHandle = ClientGoalHandle<ActionT>;
+  using Result = typename GoalHandle::Result;
+  using FeedbackCallback = typename ClientGoalHandle<ActionT>::FeedbackCallback;
+  using CancelRequest = typename ActionT::CancelGoalService::Request;
+  using CancelResponse = typename ActionT::CancelGoalService::Response;
+
+  Client(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    const std::string & action_name,
+    const rcl_action_client_options_t client_options = rcl_action_client_get_default_options()
+  )
+  : ClientBase(
+      node_base, node_graph, node_logging, action_name,
+      rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
+      client_options)
+  {
+  }
+
+  std::shared_future<typename GoalHandle::SharedPtr>
+  async_send_goal(
+    const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false)
+  {
+    // Put promise in the heap to move it around.
+    auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
+    std::shared_future<typename GoalHandle::SharedPtr> future(promise->get_future());
+    using GoalRequest = typename ActionT::GoalRequestService::Request;
+    // auto goal_request = std::make_shared<GoalRequest>();
+    // goal_request->goal_id = this->generate_goal_id();
+    // goal_request->goal = goal;
+    auto goal_request = std::make_shared<GoalRequest>(goal);
+    goal_request->action_goal_id.uuid = this->generate_goal_id();
+    this->send_goal_request(
+      std::static_pointer_cast<void>(goal_request),
+      [this, goal_request, callback, ignore_result, promise](
+        std::shared_ptr<void> response) mutable
+      {
+        using GoalResponse = typename ActionT::GoalRequestService::Response;
+        auto goal_response = std::static_pointer_cast<GoalResponse>(response);
+        if (!goal_response->accepted) {
+          promise->set_value(nullptr);
+          return;
+        }
+        GoalInfo goal_info;
+        // goal_info.goal_id = goal_request->goal_id;
+        goal_info.goal_id.uuid = goal_request->action_goal_id.uuid;
+        goal_info.stamp = goal_response->stamp;
+        // Do not use std::make_shared as friendship cannot be forwarded.
+        std::shared_ptr<GoalHandle> goal_handle(new GoalHandle(goal_info, callback));
+        if (!ignore_result) {
+          try {
+            this->make_result_aware(goal_handle);
+          } catch (...) {
+            promise->set_exception(std::current_exception());
+            return;
+          }
+        }
+        std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+        goal_handles_[goal_handle->get_goal_id()] = goal_handle;
+        promise->set_value(goal_handle);
+      });
+    return future;
+  }
+
+  std::shared_future<Result>
+  async_get_result(typename GoalHandle::SharedPtr goal_handle)
+  {
+    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
+      throw exceptions::UnknownGoalHandleError();
+    }
+    // If the user chose to ignore the result before, then ask the server for the result now.
+    if (!goal_handle->is_result_aware()) {
+      this->make_result_aware(goal_handle);
+    }
+    return goal_handle->async_result();
+  }
+
+  std::shared_future<bool>
+  async_cancel_goal(typename GoalHandle::SharedPtr goal_handle)
+  {
+    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
+      throw exceptions::UnknownGoalHandleError();
+    }
+    // Put promise in the heap to move it around.
+    auto promise = std::make_shared<std::promise<bool>>();
+    std::shared_future<bool> future(promise->get_future());
+    auto cancel_request = std::make_shared<CancelRequest>();
+    // cancel_request->goal_info.goal_id = goal_handle->get_goal_id();
+    cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id();
+    this->send_cancel_request(
+      std::static_pointer_cast<void>(cancel_request),
+      [goal_handle, promise](std::shared_ptr<void> response) mutable
+      {
+        auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
+        bool goal_canceled = false;
+        if (!cancel_response->goals_canceling.empty()) {
+          const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0];
+          // goal_canceled = (canceled_goal_info.goal_id == goal_handle->get_goal_id());
+          goal_canceled = (canceled_goal_info.goal_id.uuid == goal_handle->get_goal_id());
+        }
+        promise->set_value(goal_canceled);
+      });
+    return future;
+  }
+
+  std::shared_future<typename CancelResponse::SharedPtr>
+  async_cancel_all_goals()
+  {
+    auto cancel_request = std::make_shared<CancelRequest>();
+    // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
+    std::fill(
+      cancel_request->goal_info.goal_id.uuid.begin(),
+      cancel_request->goal_info.goal_id.uuid.end(), 0u);
+    return async_cancel(cancel_request);
+  }
+
+  std::shared_future<typename CancelResponse::SharedPtr>
+  async_cancel_goals_before(const rclcpp::Time & stamp)
+  {
+    auto cancel_request = std::make_shared<CancelRequest>();
+    // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
+    std::fill(
+      cancel_request->goal_info.goal_id.uuid.begin(),
+      cancel_request->goal_info.goal_id.uuid.end(), 0u);
+    cancel_request->goal_info.stamp = stamp;
+    return async_cancel(cancel_request);
+  }
+
+  virtual
+  ~Client()
+  {
+    std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+    auto it = goal_handles_.begin();
+    while (it != goal_handles_.end()) {
+      it->second->invalidate();
+      it = goal_handles_.erase(it);
+    }
+  }
+
+private:
+  /// \internal
+  std::shared_ptr<void>
+  create_goal_response() const override
+  {
+    using GoalResponse = typename ActionT::GoalRequestService::Response;
+    return std::shared_ptr<void>(new GoalResponse());
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_result_response() const override
+  {
+    using GoalResultResponse = typename ActionT::GoalResultService::Response;
+    return std::shared_ptr<void>(new GoalResultResponse());
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_cancel_response() const override
+  {
+    return std::shared_ptr<void>(new CancelResponse());
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_feedback_message() const override
+  {
+    // using FeedbackMessage = typename ActionT::FeedbackMessage;
+    // return std::shared_ptr<void>(new FeedbackMessage());
+    return std::shared_ptr<void>(new Feedback());
+  }
+
+  /// \internal
+  void
+  handle_feedback_message(std::shared_ptr<void> message) override
+  {
+    std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+    // using FeedbackMessage = typename ActionT::FeedbackMessage;
+    // typename FeedbackMessage::SharedPtr feedback_message =
+    //   std::static_pointer_cast<FeedbackMessage>(message);
+    typename Feedback::SharedPtr feedback_message =
+      std::static_pointer_cast<Feedback>(message);
+    // const GoalID & goal_id = feedback_message->goal_id;
+    const GoalID & goal_id = feedback_message->action_goal_id.uuid;
+    if (goal_handles_.count(goal_id) == 0) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "Received feedback for unknown goal. Ignoring...");
+      return;
+    }
+    typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
+    // goal_handle->call_feedback_callback(goal_handle, feedback_message->feedback);
+    goal_handle->call_feedback_callback(goal_handle, feedback_message);
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_status_message() const override
+  {
+    using GoalStatusMessage = typename ActionT::GoalStatusMessage;
+    return std::shared_ptr<void>(new GoalStatusMessage());
+  }
+
+  /// \internal
+  void
+  handle_status_message(std::shared_ptr<void> message) override
+  {
+    std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+    using GoalStatusMessage = typename ActionT::GoalStatusMessage;
+    auto status_message = std::static_pointer_cast<GoalStatusMessage>(message);
+    for (const GoalStatus & status : status_message->status_list) {
+      // const GoalID & goal_id = status.goal_info.goal_id;
+      const GoalID & goal_id = status.goal_info.goal_id.uuid;
+      if (goal_handles_.count(goal_id) == 0) {
+        RCLCPP_DEBUG(
+          this->get_logger(),
+          "Received status for unknown goal. Ignoring...");
+        continue;
+      }
+      typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
+      goal_handle->set_status(status.status);
+      const int8_t goal_status = goal_handle->get_status();
+      if (
+        goal_status == GoalStatus::STATUS_SUCCEEDED ||
+        goal_status == GoalStatus::STATUS_CANCELED ||
+        goal_status == GoalStatus::STATUS_ABORTED)
+      {
+        goal_handles_.erase(goal_id);
+      }
+    }
+  }
+
+  /// \internal
+  void
+  make_result_aware(typename GoalHandle::SharedPtr goal_handle)
+  {
+    using GoalResultRequest = typename ActionT::GoalResultService::Request;
+    auto goal_result_request = std::make_shared<GoalResultRequest>();
+    // goal_result_request.goal_id = goal_handle->get_goal_id();
+    goal_result_request->action_goal_id.uuid = goal_handle->get_goal_id();
+    this->send_result_request(
+      std::static_pointer_cast<void>(goal_result_request),
+      [goal_handle, this](std::shared_ptr<void> response) mutable
+      {
+        // Wrap the response in a struct with the fields a user cares about
+        Result result;
+        using GoalResultResponse = typename ActionT::GoalResultService::Response;
+        result.response = std::static_pointer_cast<GoalResultResponse>(response);
+        result.goal_id = goal_handle->get_goal_id();
+        result.code = static_cast<ResultCode>(result.response->action_status);
+        goal_handle->set_result(result);
+        std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+        goal_handles_.erase(goal_handle->get_goal_id());
+      });
+    goal_handle->set_result_awareness(true);
+  }
+
+  /// \internal
+  std::shared_future<typename CancelResponse::SharedPtr>
+  async_cancel(typename CancelRequest::SharedPtr cancel_request)
+  {
+    // Put promise in the heap to move it around.
+    auto promise = std::make_shared<std::promise<typename CancelResponse::SharedPtr>>();
+    std::shared_future<typename CancelResponse::SharedPtr> future(promise->get_future());
+    this->send_cancel_request(
+      std::static_pointer_cast<void>(cancel_request),
+      [promise](std::shared_ptr<void> response) mutable
+      {
+        auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
+        promise->set_value(cancel_response);
+      });
+    return future;
+  }
+
+  std::map<GoalID, typename GoalHandle::SharedPtr> goal_handles_;
+  std::mutex goal_handles_mutex_;
+};
+}  // namespace rclcpp_action
+
+#endif  // RCLCPP_ACTION__CLIENT_HPP_

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -0,0 +1,130 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_
+#define RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_
+
+#include <rcl_action/action_client.h>
+
+#include <action_msgs/msg/goal_status.hpp>
+#include <rclcpp/macros.hpp>
+#include <rclcpp/time.hpp>
+
+#include <functional>
+#include <future>
+#include <memory>
+#include <mutex>
+
+#include "rclcpp_action/types.hpp"
+#include "rclcpp_action/visibility_control.hpp"
+
+namespace rclcpp_action
+{
+/// The possible statuses that an action goal can finish with.
+enum class ResultCode : int8_t
+{
+  UNKNOWN = action_msgs::msg::GoalStatus::STATUS_UNKNOWN,
+  SUCCEEDED = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED,
+  CANCELED = action_msgs::msg::GoalStatus::STATUS_CANCELED,
+  ABORTED = action_msgs::msg::GoalStatus::STATUS_ABORTED
+};
+
+
+// Forward declarations
+template<typename ActionT>
+class Client;
+
+template<typename ActionT>
+class ClientGoalHandle
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle)
+
+  // A wrapper that defines the result of an action
+  typedef struct Result
+  {
+    /// The unique identifier of the goal
+    GoalID goal_id;
+    /// A status to indicate if the goal was canceled, aborted, or suceeded
+    ResultCode code;
+    /// User defined fields sent back with an action
+    typename ActionT::Result::SharedPtr response;
+  } Result;
+
+  using Feedback = typename ActionT::Feedback;
+  using FeedbackCallback =
+    std::function<void (
+        typename ClientGoalHandle<ActionT>::SharedPtr, const std::shared_ptr<const Feedback>)>;
+
+  virtual ~ClientGoalHandle();
+
+  const GoalID &
+  get_goal_id() const;
+
+  rclcpp::Time
+  get_goal_stamp() const;
+
+  std::shared_future<Result>
+  async_result();
+
+  int8_t
+  get_status();
+
+  bool
+  is_feedback_aware();
+
+  bool
+  is_result_aware();
+
+private:
+  // The templated Client creates goal handles
+  friend Client<ActionT>;
+
+  ClientGoalHandle(const GoalInfo & info, FeedbackCallback callback);
+
+  void
+  set_feedback_callback(FeedbackCallback callback);
+
+  void
+  call_feedback_callback(
+    typename ClientGoalHandle<ActionT>::SharedPtr shared_this,
+    typename std::shared_ptr<const Feedback> feedback_message);
+
+  void
+  set_result_awareness(bool awareness);
+
+  void
+  set_status(int8_t status);
+
+  void
+  set_result(const Result & result);
+
+  void
+  invalidate();
+
+  GoalInfo info_;
+
+  bool is_result_aware_{false};
+  std::promise<Result> result_promise_;
+  std::shared_future<Result> result_future_;
+
+  FeedbackCallback feedback_callback_{nullptr};
+  int8_t status_{GoalStatus::STATUS_ACCEPTED};
+
+  std::mutex handle_mutex_;
+};
+}  // namespace rclcpp_action
+
+#include <rclcpp_action/client_goal_handle_impl.hpp>  // NOLINT(build/include_order)
+#endif  // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -0,0 +1,155 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_
+#define RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_
+
+#include <rcl_action/types.h>
+
+#include <memory>
+
+#include "rclcpp/logging.hpp"
+#include "rclcpp_action/client_goal_handle.hpp"
+#include "rclcpp_action/exceptions.hpp"
+
+namespace rclcpp_action
+{
+
+template<typename ActionT>
+ClientGoalHandle<ActionT>::ClientGoalHandle(
+  const GoalInfo & info, FeedbackCallback callback)
+: info_(info), result_future_(result_promise_.get_future()), feedback_callback_(callback)
+{
+}
+
+template<typename ActionT>
+ClientGoalHandle<ActionT>::~ClientGoalHandle()
+{
+}
+
+template<typename ActionT>
+const GoalID &
+ClientGoalHandle<ActionT>::get_goal_id() const
+{
+  // return info_.goal_id;
+  return info_.goal_id.uuid;
+}
+
+template<typename ActionT>
+rclcpp::Time
+ClientGoalHandle<ActionT>::get_goal_stamp() const
+{
+  return info_.stamp;
+}
+
+template<typename ActionT>
+std::shared_future<typename ClientGoalHandle<ActionT>::Result>
+ClientGoalHandle<ActionT>::async_result()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  if (!is_result_aware_) {
+    throw exceptions::UnawareGoalHandleError();
+  }
+  return result_future_;
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_result(const Result & result)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  status_ = static_cast<int8_t>(result.code);
+  result_promise_.set_value(result);
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_feedback_callback(FeedbackCallback callback)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  feedback_callback_ = callback;
+}
+
+template<typename ActionT>
+int8_t
+ClientGoalHandle<ActionT>::get_status()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  return status_;
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_status(int8_t status)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  status_ = status;
+}
+
+template<typename ActionT>
+bool
+ClientGoalHandle<ActionT>::is_feedback_aware()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  return feedback_callback_ != nullptr;
+}
+
+template<typename ActionT>
+bool
+ClientGoalHandle<ActionT>::is_result_aware()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  return is_result_aware_;
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_result_awareness(bool awareness)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  is_result_aware_ = awareness;
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::invalidate()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  status_ = GoalStatus::STATUS_UNKNOWN;
+  result_promise_.set_exception(std::make_exception_ptr(
+      exceptions::UnawareGoalHandleError()));
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::call_feedback_callback(
+  typename ClientGoalHandle<ActionT>::SharedPtr shared_this,
+  typename std::shared_ptr<const Feedback> feedback_message)
+{
+  if (shared_this.get() != this) {
+    RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle.");
+    return;
+  }
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  if (nullptr == feedback_callback_) {
+    // Normal, some feedback messages may arrive after the goal result.
+    RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it.");
+    return;
+  }
+  feedback_callback_(shared_this, feedback_message);
+}
+
+}  // namespace rclcpp_action
+
+#endif  // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_

rclcpp_action/include/rclcpp_action/create_client.hpp

@@ -0,0 +1,78 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP_ACTION__CREATE_CLIENT_HPP_
+#define RCLCPP_ACTION__CREATE_CLIENT_HPP_
+
+#include <rclcpp/node.hpp>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp_action/client.hpp"
+#include "rclcpp_action/visibility_control.hpp"
+
+namespace rclcpp_action
+{
+template<typename ActionT>
+typename Client<ActionT>::SharedPtr
+create_client(
+  rclcpp::Node::SharedPtr node,
+  const std::string & name,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
+{
+  std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
+    node->get_node_waitables_interface();
+  std::weak_ptr<rclcpp::callback_group::CallbackGroup> weak_group = group;
+  bool group_is_null = (nullptr == group.get());
+
+  auto deleter = [weak_node, weak_group, group_is_null](Client<ActionT> * ptr)
+    {
+      if (nullptr == ptr) {
+        return;
+      }
+      auto shared_node = weak_node.lock();
+      if (!shared_node) {
+        return;
+      }
+      // API expects a shared pointer, give it one with a deleter that does nothing.
+      std::shared_ptr<Client<ActionT>> fake_shared_ptr(ptr, [](Client<ActionT> *) {});
+
+      if (group_is_null) {
+        // Was added to default group
+        shared_node->remove_waitable(fake_shared_ptr, nullptr);
+      } else {
+        // Was added to a specfic group
+        auto shared_group = weak_group.lock();
+        if (shared_group) {
+          shared_node->remove_waitable(fake_shared_ptr, shared_group);
+        }
+      }
+      delete ptr;
+    };
+
+  std::shared_ptr<Client<ActionT>> action_client(
+    new Client<ActionT>(
+      node->get_node_base_interface(),
+      node->get_node_graph_interface(),
+      node->get_node_logging_interface(),
+      name),
+    deleter);
+
+  node->get_node_waitables_interface()->add_waitable(action_client, group);
+  return action_client;
+}
+}  // namespace rclcpp_action
+
+#endif  // RCLCPP_ACTION__CREATE_CLIENT_HPP_