another wip

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

.gitignore

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

rclcpp/CHANGELOG.rst

@@ -2,6 +2,24 @@
 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>`_)

rclcpp/CMakeLists.txt

@@ -40,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
@@ -52,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
@@ -62,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
@@ -396,6 +401,17 @@ if(BUILD_TESTING)
       "rcl")
     target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME})
   endif()
+
+  ament_add_gtest(test_local_parameters test/test_local_parameters.cpp)
+  if(TARGET test_local_parameters)
+    target_include_directories(test_local_parameters PUBLIC
+      ${rcl_interfaces_INCLUDE_DIRS}
+      ${rmw_INCLUDE_DIRS}
+      ${rosidl_generator_cpp_INCLUDE_DIRS}
+      ${rosidl_typesupport_cpp_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_local_parameters ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_package(

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/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_;
 };
@@ -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/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/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
@@ -237,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.
@@ -353,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

@@ -51,7 +51,7 @@ public:
    */
   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

@@ -49,14 +49,14 @@ template<typename FunctionT>
 struct function_traits
 {
   using arguments = typename tuple_tail<
-    typename function_traits<decltype( & FunctionT::operator())>::arguments>::type;
+    typename function_traits<decltype( &FunctionT::operator())>::arguments>::type;
 
   static constexpr std::size_t arity = std::tuple_size<arguments>::value;
 
   template<std::size_t N>
   using argument_type = typename std::tuple_element<N, arguments>::type;
 
-  using return_type = typename function_traits<decltype( & FunctionT::operator())>::return_type;
+  using return_type = typename function_traits<decltype( &FunctionT::operator())>::return_type;
 };
 
 // Free functions
@@ -81,7 +81,7 @@ struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(A
 // std::bind for object methods
 template<typename ClassT, typename ReturnTypeT, typename ... Args, typename ... FArgs>
 #if defined _LIBCPP_VERSION  // libc++ (Clang)
-struct function_traits<std::__1::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
+struct function_traits<std::__bind<ReturnTypeT (ClassT::*)(Args ...), FArgs ...>>
 #elif defined _GLIBCXX_RELEASE  // glibc++ (GNU C++ >= 7.1)
 struct function_traits<std::_Bind<ReturnTypeT(ClassT::*(FArgs ...))(Args ...)>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
@@ -99,7 +99,7 @@ struct function_traits<
 // std::bind for free functions
 template<typename ReturnTypeT, typename ... Args, typename ... FArgs>
 #if defined _LIBCPP_VERSION  // libc++ (Clang)
-struct function_traits<std::__1::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
+struct function_traits<std::__bind<ReturnTypeT( &)(Args ...), FArgs ...>>
 #elif defined __GLIBCXX__  // glibc++ (GNU C++)
 struct function_traits<std::_Bind<ReturnTypeT(*(FArgs ...))(Args ...)>>
 #elif defined _MSC_VER  // MS Visual Studio

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/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
 {
@@ -52,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;
@@ -76,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;
 
@@ -113,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/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"
@@ -93,6 +95,7 @@ public:
    * 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.
+   * \param[in] allow_undeclared_parameters True to allow any parameter name to be set on the node.
    */
   RCLCPP_PUBLIC
   Node(
@@ -103,7 +106,8 @@ public:
     const std::vector<Parameter> & initial_parameters,
     bool use_global_arguments = true,
     bool use_intra_process_comms = false,
-    bool start_parameter_services = true);
+    bool start_parameter_services = true,
+    bool allow_undeclared_parameters = false);
 
   RCLCPP_PUBLIC
   virtual ~Node();
@@ -263,6 +267,36 @@ public:
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
+  /// Declare and initialize a parameter.
+  /**
+   * This method is used to declare that a parameter exists on this node.
+   * If a run-time user has provided an an initial value then it will be set in this method,
+   * otherwise the default_value will be set.
+   * \param[in] name the name of the parameter
+   * \param[in] default_value An initial value to be used if a run-time user did not override it.
+   * \param[in] read_only if True then this parameter may not be changed after initialization.
+   * \throws std::runtime_error if parameter has already been declared.
+   * \throws std::runtime_error if a parameter name is invalid.
+   * \throws rclcpp::exceptions::InvalidParameterValueException if initial value fails to be set.
+   */
+  RCLCPP_PUBLIC
+  void
+  declare_parameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+    bool read_only = false);
+
+  /// Declare and initialize a parameter with a type.
+  /**
+   * See the non-templated declare_parameter() on this class for details.
+   */
+  template<typename ParameterT>
+  void
+  declare_parameter(
+    const std::string & name,
+    const ParameterT & default_value,
+    bool read_only = false);
+
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
   set_parameters(const std::vector<rclcpp::Parameter> & parameters);
@@ -277,6 +311,20 @@ public:
     const std::string & name,
     const ParameterT & value);
 
+  /// Set a map of parameters with the same prefix.
+  /**
+   * For each key in the map, a parameter with a name of "name.key" will be set
+   * to the value in the map.
+   *
+   * \param[in] name The prefix of the parameters to set.
+   * \param[in] values The parameters to set in the given prefix.
+   */
+  template<typename MapValueT>
+  void
+  set_parameters_if_not_set(
+    const std::string & name,
+    const std::map<std::string, MapValueT> & values);
+
   RCLCPP_PUBLIC
   std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & names) const;
@@ -303,6 +351,24 @@ public:
   bool
   get_parameter(const std::string & name, ParameterT & parameter) const;
 
+  /// Assign the value of the map parameter if set into the values argument.
+  /**
+   * Parameter names that are part of a map are of the form "name.member".
+   * This API gets all parameters that begin with "name", storing them into the
+   * map with the name of the parameter and their value.
+   * If there are no members in the named map, then the "values" argument is not changed.
+   *
+   * \param[in] name The prefix of the parameters to get.
+   * \param[out] values The map of output values, with one std::string,MapValueT
+   *                    per parameter.
+   * \returns true if values was changed, false otherwise
+   */
+  template<typename MapValueT>
+  bool
+  get_parameters(
+    const std::string & name,
+    std::map<std::string, MapValueT> & values) const;
+
   /// Get the parameter value, or the "alternative value" if not set, and assign it to "value".
   /**
    * If the parameter was not set, then the "value" argument is assigned
@@ -427,6 +493,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
@@ -442,11 +513,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)
 
@@ -460,8 +541,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;
 }
@@ -209,6 +210,16 @@ Node::register_param_change_callback(CallbackT && callback)
   this->node_parameters_->register_param_change_callback(std::forward<CallbackT>(callback));
 }
 
+template<typename ParameterT>
+void
+Node::declare_parameter(
+  const std::string & name,
+  const ParameterT & default_value,
+  bool read_only)
+{
+  this->declare_parameter(name, rclcpp::ParameterValue(default_value), read_only);
+}
+
 template<typename ParameterT>
 void
 Node::set_parameter_if_not_set(
@@ -223,6 +234,28 @@ Node::set_parameter_if_not_set(
   }
 }
 
+// this is a partially-specialized version of set_parameter_if_not_set above,
+// where our concrete type for ParameterT is std::map, but the to-be-determined
+// type is the value in the map.
+template<typename MapValueT>
+void
+Node::set_parameters_if_not_set(
+  const std::string & name,
+  const std::map<std::string, MapValueT> & values)
+{
+  std::vector<rclcpp::Parameter> params;
+
+  for (const auto & val : values) {
+    std::string param_name = name + "." + val.first;
+    rclcpp::Parameter parameter;
+    if (!this->get_parameter(param_name, parameter)) {
+      params.push_back(rclcpp::Parameter(param_name, val.second));
+    }
+  }
+
+  this->set_parameters(params);
+}
+
 template<typename ParameterT>
 bool
 Node::get_parameter(const std::string & name, ParameterT & value) const
@@ -236,6 +269,26 @@ Node::get_parameter(const std::string & name, ParameterT & value) const
   return result;
 }
 
+// this is a partially-specialized version of get_parameter above,
+// where our concrete type for ParameterT is std::map, but the to-be-determined
+// type is the value in the map.
+template<typename MapValueT>
+bool
+Node::get_parameters(
+  const std::string & name,
+  std::map<std::string, MapValueT> & values) const
+{
+  std::map<std::string, rclcpp::Parameter> params;
+  bool result = node_parameters_->get_parameters_by_prefix(name, params);
+  if (result) {
+    for (const auto & param : params) {
+      values[param.first] = param.second.get_value<MapValueT>();
+    }
+  }
+
+  return result;
+}
+
 template<typename ParameterT>
 bool
 Node::get_parameter_or(

rclcpp/include/rclcpp/node_interfaces/get_node_parameters_interface.hpp

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

rclcpp/include/rclcpp/node_interfaces/node_base_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,10 @@ private:
   rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
   rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_;
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
 
   rclcpp::Clock::SharedPtr ros_clock_;
-  rclcpp::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_interface.hpp

@@ -38,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

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

@@ -40,6 +40,19 @@ namespace rclcpp
 namespace node_interfaces
 {
 
+// Internal struct for holding useful info about parameters
+struct ParameterInfo
+{
+  /// True if a user called declare_parameter()
+  bool is_declared = false;
+
+  /// Current value of the parameter.
+  rclcpp::ParameterValue value;
+
+  /// A description of the parameter
+  rcl_interfaces::msg::ParameterDescriptor descriptor;
+};
+
 /// Implementation of the NodeParameters part of the Node API.
 class NodeParameters : public NodeParametersInterface
 {
@@ -51,62 +64,68 @@ 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);
+    bool start_parameter_services,
+    bool allow_undeclared_parameters);
 
   RCLCPP_PUBLIC
   virtual
   ~NodeParameters();
 
   RCLCPP_PUBLIC
-  virtual
+  void
+  declare_parameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value,
+    bool read_only) override;
+
+  RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
   set_parameters(
-    const std::vector<rclcpp::Parameter> & parameters);
+    const std::vector<rclcpp::Parameter> & parameters) override;
 
   RCLCPP_PUBLIC
-  virtual
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(
-    const std::vector<rclcpp::Parameter> & parameters);
+    const std::vector<rclcpp::Parameter> & parameters) override;
 
   RCLCPP_PUBLIC
-  virtual
   std::vector<rclcpp::Parameter>
-  get_parameters(const std::vector<std::string> & names) const;
+  get_parameters(const std::vector<std::string> & names) const override;
 
   RCLCPP_PUBLIC
-  virtual
   rclcpp::Parameter
-  get_parameter(const std::string & name) const;
+  get_parameter(const std::string & name) const override;
 
   RCLCPP_PUBLIC
-  virtual
   bool
   get_parameter(
     const std::string & name,
-    rclcpp::Parameter & parameter) const;
+    rclcpp::Parameter & parameter) const override;
+
+  RCLCPP_PUBLIC
+  bool
+  get_parameters_by_prefix(
+    const std::string & prefix,
+    std::map<std::string, rclcpp::Parameter> & parameters) const override;
 
   RCLCPP_PUBLIC
-  virtual
   std::vector<rcl_interfaces::msg::ParameterDescriptor>
-  describe_parameters(const std::vector<std::string> & names) const;
+  describe_parameters(const std::vector<std::string> & names) const override;
 
   RCLCPP_PUBLIC
-  virtual
   std::vector<uint8_t>
-  get_parameter_types(const std::vector<std::string> & names) const;
+  get_parameter_types(const std::vector<std::string> & names) const override;
 
   RCLCPP_PUBLIC
-  virtual
   rcl_interfaces::msg::ListParametersResult
-  list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
+  list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
   RCLCPP_PUBLIC
-  virtual
   void
-  register_param_change_callback(ParametersCallbackFunction callback);
+  register_param_change_callback(ParametersCallbackFunction callback) override;
 
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
@@ -115,11 +134,19 @@ private:
 
   ParametersCallbackFunction parameters_callback_ = nullptr;
 
-  std::map<std::string, rclcpp::Parameter> parameters_;
+  std::map<std::string, ParameterInfo> parameters_;
+
+  std::map<std::string, rclcpp::ParameterValue> initial_parameter_values_;
+
+  bool allow_undeclared_ = false;
 
   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

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 
+#include <map>
 #include <string>
 #include <vector>
 
@@ -37,6 +38,29 @@ class NodeParametersInterface
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface)
 
+  /// Declare and initialize a parameter.
+  /* This method is used to declare that a parameter exists on this node.
+   * If a run-time user has provided an an initial value then it will be set in this method,
+   * otherwise the default_value will be set.
+   * \param[in] name the name of the parameter
+   * \param[in] default_value An initial value to be used if a run-time user did not override it.
+   * \param[in] read_only if True then this parameter may not be changed after initialization.
+   * \throws std::runtime_error if parameter has already been declared.
+   * \throws std::runtime_error if a parameter name is invalid.
+   * \throws rclcpp::exceptions::InvalidParameterValueException if initial value fails to be set.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  declare_parameter(
+    const std::string & name,
+    const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+    bool read_only = false) = 0;
+
+  RCLCPP_PUBLIC
+  virtual
+  ~NodeParametersInterface() = default;
+
   RCLCPP_PUBLIC
   virtual
   std::vector<rcl_interfaces::msg::SetParametersResult>
@@ -85,6 +109,20 @@ public:
     const std::string & name,
     rclcpp::Parameter & parameter) const = 0;
 
+  /// Get all parameters that have the specified prefix into the parameters map.
+  /*
+   * \param[in] prefix the name of the prefix to look for.
+   * \param[out] parameters a map of parameters that matched the prefix.
+   * \return true if any parameters with the prefix exists on the node, or
+   * \return false otherwise.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  bool
+  get_parameters_by_prefix(
+    const std::string & prefix,
+    std::map<std::string, rclcpp::Parameter> & parameters) const = 0;
+
   RCLCPP_PUBLIC
   virtual
   std::vector<rcl_interfaces::msg::ParameterDescriptor>

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

@@ -28,6 +28,25 @@
 namespace rclcpp
 {
 
+class Parameter;
+
+namespace node_interfaces
+{
+struct ParameterInfo;
+}  // namespace node_interfaces
+
+namespace detail
+{
+
+// This helper function is required because you cannot do specialization on a
+// class method, so instead we specialize this template function and call it
+// from the unspecialized, but dependent, class method.
+template<typename T>
+auto
+get_value_helper(const rclcpp::Parameter * parameter);
+
+}  // namespace detail
+
 /// Structure to store an arbitrary parameter with templated get/set methods.
 class Parameter
 {
@@ -44,6 +63,9 @@ public:
   {
   }
 
+  RCLCPP_PUBLIC
+  explicit Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info);
+
   RCLCPP_PUBLIC
   ParameterType
   get_type() const;
@@ -60,6 +82,11 @@ public:
   rcl_interfaces::msg::ParameterValue
   get_value_message() const;
 
+  /// Get the internal storage for the parameter value.
+  RCLCPP_PUBLIC
+  const rclcpp::ParameterValue &
+  get_parameter_value() const;
+
   /// Get value of parameter using rclcpp::ParameterType as template argument.
   template<ParameterType ParamT>
   decltype(auto)
@@ -71,10 +98,7 @@ public:
   /// Get value of parameter using c++ types as template argument.
   template<typename T>
   decltype(auto)
-  get_value() const
-  {
-    return value_.get<T>();
-  }
+  get_value() const;
 
   RCLCPP_PUBLIC
   bool
@@ -142,6 +166,49 @@ RCLCPP_PUBLIC
 std::ostream &
 operator<<(std::ostream & os, const std::vector<Parameter> & parameters);
 
+namespace detail
+{
+
+template<typename T>
+auto
+get_value_helper(const rclcpp::Parameter * parameter)
+{
+  return parameter->get_parameter_value().get<T>();
+}
+
+// Specialization allowing Parameter::get() to return a const ref to the parameter value object.
+template<>
+inline
+auto
+get_value_helper<rclcpp::ParameterValue>(const rclcpp::Parameter * parameter)
+{
+  return parameter->get_parameter_value();
+}
+
+// Specialization allowing Parameter::get() to return a const ref to the parameter itself.
+template<>
+inline
+auto
+get_value_helper<rclcpp::Parameter>(const rclcpp::Parameter * parameter)
+{
+  // Use this labmda to ensure it's a const reference being returned (and not a copy).
+  auto type_enforcing_lambda =
+    [&parameter]() -> const rclcpp::Parameter & {
+      return *parameter;
+    };
+  return type_enforcing_lambda();
+}
+
+}  // namespace detail
+
+template<typename T>
+decltype(auto)
+Parameter::get_value() const
+{
+  // use the helper to specialize for the ParameterValue and Parameter cases.
+  return detail::get_value_helper<T>(this);
+}
+
 }  // namespace rclcpp
 
 namespace std

rclcpp/include/rclcpp/parameter_client.hpp

@@ -185,6 +185,29 @@ public:
     const std::string & remote_node_name = "",
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
+  RCLCPP_PUBLIC
+  explicit SyncParametersClient(
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  SyncParametersClient(
+    rclcpp::executor::Executor::SharedPtr executor,
+    rclcpp::Node * node,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
+  RCLCPP_PUBLIC
+  SyncParametersClient(
+    rclcpp::executor::Executor::SharedPtr executor,
+    const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+    const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+    const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+    const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+    const std::string & remote_node_name = "",
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
+
   RCLCPP_PUBLIC
   std::vector<rclcpp::Parameter>
   get_parameters(const std::vector<std::string> & parameter_names);
@@ -268,7 +291,7 @@ public:
 
 private:
   rclcpp::executor::Executor::SharedPtr executor_;
-  rclcpp::Node::SharedPtr node_;
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
   AsyncParametersClient::SharedPtr async_parameters_client_;
 };
 

rclcpp/include/rclcpp/parameter_value.hpp

@@ -28,7 +28,8 @@
 
 namespace rclcpp
 {
-enum ParameterType
+
+enum ParameterType : uint8_t
 {
   PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET,
   PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL,
@@ -45,11 +46,11 @@ enum ParameterType
 /// Return the name of a parameter type
 RCLCPP_PUBLIC
 std::string
-to_string(const ParameterType type);
+to_string(ParameterType type);
 
 RCLCPP_PUBLIC
 std::ostream &
-operator<<(std::ostream & os, const ParameterType type);
+operator<<(std::ostream & os, ParameterType type);
 
 /// Indicate the parameter type does not match the expected type.
 class ParameterTypeException : public std::runtime_error

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");
       }
@@ -305,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

@@ -119,7 +119,7 @@ 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().str);
             rcl_reset_error();

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,7 +206,7 @@ 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().str);
@@ -192,7 +215,7 @@ public:
     }
 
     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().str);
@@ -201,7 +224,7 @@ public:
     }
 
     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().str);
@@ -210,7 +233,7 @@ public:
     }
 
     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().str);
@@ -219,7 +242,7 @@ public:
     }
 
     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",
@@ -227,6 +250,15 @@ public:
         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;
+      }
+    }
     return true;
   }
 
@@ -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/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,6 +77,12 @@ 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;

rclcpp/include/rclcpp/timer.hpp

@@ -24,6 +24,7 @@
 #include <utility>
 
 #include "rclcpp/clock.hpp"
+#include "rclcpp/context.hpp"
 #include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/rate.hpp"
@@ -45,7 +46,10 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase)
 
   RCLCPP_PUBLIC
-  explicit TimerBase(Clock::SharedPtr clock, std::chrono::nanoseconds period);
+  explicit TimerBase(
+    Clock::SharedPtr clock,
+    std::chrono::nanoseconds period,
+    rclcpp::Context::SharedPtr context);
 
   RCLCPP_PUBLIC
   ~TimerBase();
@@ -114,9 +118,10 @@ public:
    * \param[in] callback User-specified callback function.
    */
   explicit GenericTimer(
-    Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback
+    Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback,
+    rclcpp::Context::SharedPtr context
   )
-  : TimerBase(clock, period), callback_(std::forward<FunctorT>(callback))
+  : TimerBase(clock, period, context), callback_(std::forward<FunctorT>(callback))
   {
   }
 
@@ -128,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) {
@@ -165,8 +170,8 @@ public:
     callback_(*this);
   }
 
-  virtual bool
-  is_steady()
+  bool
+  is_steady() override
   {
     return clock_->get_clock_type() == RCL_STEADY_TIME;
   }
@@ -189,9 +194,12 @@ class WallTimer : public GenericTimer<FunctorT>
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(WallTimer)
 
-  explicit WallTimer(std::chrono::nanoseconds period, FunctorT && callback)
+  WallTimer(
+    std::chrono::nanoseconds period,
+    FunctorT && callback,
+    rclcpp::Context::SharedPtr context)
   : GenericTimer<FunctorT>(
-      std::make_shared<Clock>(RCL_STEADY_TIME), period, std::move(callback))
+      std::make_shared<Clock>(RCL_STEADY_TIME), period, std::move(callback), context)
   {}
 
 protected:

rclcpp/include/rclcpp/unused_parameters_checker.hpp

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

rclcpp/include/rclcpp/utilities.hpp

@@ -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,65 +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. */
-RCLCPP_PUBLIC
-bool
-ok();
-
-/// Returns true if init() has already been called.
-/** \return True if init() has been called, false otherwise. */
-RCLCPP_PUBLIC
-bool
-is_initialized();
-
-/// 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.
 /**
- * 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.
+ * 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
-rcl_guard_condition_t *
-get_sigint_guard_condition(rcl_wait_set_t * wait_set);
+bool
+ok(rclcpp::Context::SharedPtr context = nullptr);
 
-/// Release the previously allocated guard condition that manages the signal handler.
+/// Return true if init() has already been called for the given context.
 /**
- * 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().
+ *
+ * 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
+bool
+is_initialized(rclcpp::Context::SharedPtr context = nullptr);
+
+/// Shutdown rclcpp context, invalidating it for derived entities.
+/**
+ * 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.
 /**

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.6.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/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,17 +37,20 @@ 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"),
+            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();
         }
@@ -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/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/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,27 +41,27 @@ 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().str);
+  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();
 
-  if (rcl_wait_set_init(
-      &wait_set_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK)
-  {
+  // Store the context for later use.
+  context_ = args.context;
+
+  ret = rcl_wait_set_init(&wait_set_, 0, 2, 0, 0, 0, context_->get_rcl_context().get(), allocator);
+  if (RCL_RET_OK != ret) {
     RCUTILS_LOG_ERROR_NAMED(
       "rclcpp",
       "failed to create wait set: %s", rcl_get_error_string().str);
@@ -102,9 +103,8 @@ Executor::~Executor()
     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
@@ -281,6 +281,9 @@ 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
@@ -427,6 +430,7 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
     throw std::runtime_error("Couldn't clear wait set");
   }
 
+  // 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(),
@@ -549,6 +553,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;
 }

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/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
@@ -69,6 +79,7 @@ GraphListener::start_if_not_started()
       0,  // number_of_timers
       0,  // number_of_clients
       0,  // number_of_services
+      this->parent_context_->get_rcl_context().get(),
       rcl_get_default_allocator());
     if (RCL_RET_OK != ret) {
       throw_from_rcl_error(ret, "failed to initialize wait set");
@@ -81,7 +92,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,8 +142,10 @@ 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(&wait_set_, 0, node_graph_interfaces_.size() + 2, 0, 0, 0);
+    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");
       }
@@ -142,17 +156,21 @@ GraphListener::run_loop()
       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 +180,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 +195,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 +340,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 +349,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 +370,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;
@@ -58,7 +60,8 @@ Node::Node(
   const std::vector<rclcpp::Parameter> & initial_parameters,
   bool use_global_arguments,
   bool use_intra_process_comms,
-  bool start_parameter_services)
+  bool start_parameter_services,
+  bool allow_undeclared_parameters)
 : node_base_(new rclcpp::node_interfaces::NodeBase(
       node_name, namespace_, context, arguments, use_global_arguments)),
   node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
@@ -66,20 +69,33 @@ 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,
+      allow_undeclared_parameters
+    )),
+  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)
 {
 }
@@ -118,6 +134,15 @@ Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group)
   return node_base_->callback_group_in_node(group);
 }
 
+void
+Node::declare_parameter(
+  const std::string & name,
+  const rclcpp::ParameterValue & default_value,
+  bool read_only)
+{
+  return this->node_parameters_->declare_parameter(name, default_value, read_only);
+}
+
 std::vector<rcl_interfaces::msg::SetParametersResult>
 Node::set_parameters(
   const std::vector<rclcpp::Parameter> & parameters)
@@ -253,6 +278,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()
 {
@@ -276,3 +313,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");
   }
@@ -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();

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

@@ -30,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)
 {}
@@ -334,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

@@ -36,9 +36,12 @@ 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)
+  bool start_parameter_services,
+  bool allow_undeclared_parameters)
+: allow_undeclared_(allow_undeclared_parameters), node_clock_(node_clock)
 {
   using MessageT = rcl_interfaces::msg::ParameterEvent;
   using PublisherT = rclcpp::Publisher<MessageT>;
@@ -64,7 +67,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
@@ -93,7 +96,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));
 
@@ -104,14 +108,12 @@ 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;
-  } else {
-    combined_name = node_namespace + '/' + node_name;
-  }
 
-  std::map<std::string, rclcpp::Parameter> parameters;
+  if ('/' == node_namespace.at(node_namespace.size() - 1)) {
+    combined_name_ = node_namespace + node_name;
+  } else {
+    combined_name_ = node_namespace + '/' + node_name;
+  }
 
   // TODO(sloretz) use rcl to parse yaml when circular dependency is solved
   // See https://github.com/ros2/rcl/issues/252
@@ -130,32 +132,26 @@ NodeParameters::NodeParameters(
 
     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;
     }
 
     // Combine parameter yaml files, overwriting values in older ones
     for (auto & param : iter->second) {
-      parameters[param.get_name()] = param;
+      initial_parameter_values_[param.get_name()] =
+        rclcpp::ParameterValue(param.get_value_message());
     }
   }
 
   // initial values passed to constructor overwrite yaml file sources
   for (auto & param : initial_parameters) {
-    parameters[param.get_name()] = param;
+    initial_parameter_values_[param.get_name()] =
+      rclcpp::ParameterValue(param.get_value_message());
   }
 
-  std::vector<rclcpp::Parameter> combined_values;
-  combined_values.reserve(parameters.size());
-  for (auto & kv : parameters) {
-    combined_values.emplace_back(kv.second);
-  }
-
-  // TODO(sloretz) store initial values and use them when a parameter is created ros2/rclcpp#475
-  // Set initial parameter values
-  if (!combined_values.empty()) {
-    rcl_interfaces::msg::SetParametersResult result = set_parameters_atomically(combined_values);
+  if (!initial_parameters.empty() && allow_undeclared_) {
+    rcl_interfaces::msg::SetParametersResult result = set_parameters_atomically(initial_parameters);
     if (!result.successful) {
       throw std::runtime_error("Failed to set initial parameters");
     }
@@ -165,6 +161,59 @@ NodeParameters::NodeParameters(
 NodeParameters::~NodeParameters()
 {}
 
+void
+NodeParameters::declare_parameter(
+  const std::string & name,
+  const rclcpp::ParameterValue & default_value,
+  bool read_only)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  // TODO(sloretz) parameter name validation
+  if (name.empty()) {
+    throw std::runtime_error("parameter name must not be empty");
+  }
+
+  // Error if this parameter has already been declared and is different
+  auto param_iter = parameters_.find(name);
+  if (param_iter != parameters_.end() && param_iter->second.is_declared) {
+    if (
+      param_iter->second.descriptor.type != default_value.get_type() ||
+      param_iter->second.descriptor.read_only != read_only)
+    {
+      throw std::runtime_error("parameter '" + name + "' exists with conflicting description");
+    }
+  }
+
+  // Check if run-time user passed an initial value, else use the default.
+  rclcpp::ParameterValue initial_value = default_value;
+  auto value_iter = initial_parameter_values_.find(name);
+  if (value_iter != initial_parameter_values_.end()) {
+    initial_value = value_iter->second;
+  }
+
+  // Save a description of the parameter
+  rcl_interfaces::msg::ParameterDescriptor desc;
+  desc.name = name;
+  desc.type = initial_value.get_type();
+  desc.read_only = read_only;
+
+  rclcpp::node_interfaces::ParameterInfo pinfo;
+  pinfo.is_declared = true;
+  pinfo.value = initial_value;
+  pinfo.descriptor = desc;
+
+  // Add declared parameters to storage, even when they're not set.
+  parameters_[name] = pinfo;
+
+  // If it has an actual value then add it to 'new_parameters' event
+  if (rclcpp::ParameterType::PARAMETER_NOT_SET != initial_value.get_type()) {
+    auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
+    parameter_event->new_parameters.push_back(
+      rclcpp::Parameter(name, initial_value).to_parameter_msg());
+    events_publisher_->publish(parameter_event);
+  }
+}
+
 std::vector<rcl_interfaces::msg::SetParametersResult>
 NodeParameters::set_parameters(
   const std::vector<rclcpp::Parameter> & parameters)
@@ -182,9 +231,11 @@ NodeParameters::set_parameters_atomically(
   const std::vector<rclcpp::Parameter> & parameters)
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  std::map<std::string, rclcpp::Parameter> tmp_map;
+  std::map<std::string, rclcpp::node_interfaces::ParameterInfo> tmp_map;
   auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
 
+  parameter_event->node = combined_name_;
+
   // TODO(jacquelinekay): handle parameter constraints
   rcl_interfaces::msg::SetParametersResult result;
   if (parameters_callback_) {
@@ -197,37 +248,83 @@ NodeParameters::set_parameters_atomically(
     return result;
   }
 
+  std::vector<std::string> to_delete;
+
   for (auto p : parameters) {
-    if (p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
-      if (parameters_.find(p.get_name()) != parameters_.end()) {
-        // case: parameter was set before, and input is "NOT_SET"
-        // therefore we will erase the parameter from parameters_ later
+    auto param_iter = parameters_.find(p.get_name());
+    bool exists = parameters_.end() != param_iter;
+    bool want_to_delete = p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET;
+
+    if (exists) {
+      if (param_iter->second.descriptor.read_only) {
+        result.successful = false;
+        result.reason = "read_only parameter: '" + p.get_name() + "'";
+        return result;
+      }
+      if (want_to_delete) {
         parameter_event->deleted_parameters.push_back(p.to_parameter_msg());
+        if (param_iter->second.is_declared) {
+          // clear declared parameter value, but don't delete it
+          rclcpp::node_interfaces::ParameterInfo cleared_param_info = param_iter->second;
+          cleared_param_info.value = rclcpp::ParameterValue();
+          tmp_map[p.get_name()] = cleared_param_info;
+        } else {
+          // Truly delete an undeclared parameter
+          to_delete.push_back(p.get_name());
+        }
+      } else {
+        if (param_iter->second.value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
+          // case: setting a value on a declared parameter that currently is unset
+          parameter_event->new_parameters.push_back(p.to_parameter_msg());
+        } else {
+          // case: changing a parameter value
+          parameter_event->changed_parameters.push_back(p.to_parameter_msg());
+        }
+        rclcpp::node_interfaces::ParameterInfo changed_param_info = param_iter->second;
+        // TODO(sloretz) Add accessor for ParameterValue on Parameter class
+        changed_param_info.value = rclcpp::ParameterValue(p.get_value_message());
+        tmp_map[p.get_name()] = changed_param_info;
       }
     } else {
-      if (parameters_.find(p.get_name()) == parameters_.end()) {
-        // case: parameter not set before, and input is something other than "NOT_SET"
-        parameter_event->new_parameters.push_back(p.to_parameter_msg());
+      // case: parameter does not exist already
+      if (!allow_undeclared_) {
+        result.successful = false;
+        result.reason = "undeclared parameter: '" + p.get_name() + "'";
+        return result;
+      } else if (want_to_delete) {
+        result.successful = false;
+        result.reason = "deleting parameter: '" + p.get_name() + "' that does not exist";
+        return result;
       } else {
-        // case: parameter was set before, and input is something other than "NOT_SET"
-        parameter_event->changed_parameters.push_back(p.to_parameter_msg());
+        // Create new undeclared parameter
+        parameter_event->new_parameters.push_back(p.to_parameter_msg());
+
+        rcl_interfaces::msg::ParameterDescriptor desc;
+        desc.name = p.get_name();
+        desc.type = p.get_type();
+        desc.read_only = false;
+
+        rclcpp::node_interfaces::ParameterInfo new_param_info;
+        new_param_info.is_declared = false;
+        // TODO(sloretz) Add accessor for ParameterValue on Parameter class
+        new_param_info.value = rclcpp::ParameterValue(p.get_value_message());
+        new_param_info.descriptor = desc;
+
+        tmp_map[p.get_name()] = new_param_info;
       }
-      tmp_map[p.get_name()] = p;
     }
   }
   // std::map::insert will not overwrite elements, so we'll keep the new
   // ones and add only those that already exist in the Node's internal map
   tmp_map.insert(parameters_.begin(), parameters_.end());
 
-  // remove explicitly deleted parameters
-  for (auto p : parameters) {
-    if (p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
-      tmp_map.erase(p.get_name());
-    }
+  // remove truly deleted parameters
+  for (const std::string & param_name : to_delete) {
+    tmp_map.erase(param_name);
   }
 
   std::swap(tmp_map, parameters_);
-
+  parameter_event->stamp = node_clock_->get_clock()->now();
   events_publisher_->publish(parameter_event);
 
   return result;
@@ -241,11 +338,11 @@ NodeParameters::get_parameters(const std::vector<std::string> & names) const
 
   for (auto & name : names) {
     if (std::any_of(parameters_.cbegin(), parameters_.cend(),
-      [&name](const std::pair<std::string, rclcpp::Parameter> & kv) {
+      [&name](const std::pair<std::string, rclcpp::node_interfaces::ParameterInfo> & kv) {
         return name == kv.first;
       }))
     {
-      results.push_back(parameters_.at(name));
+      results.emplace_back(name, parameters_.at(name).value);
     }
   }
   return results;
@@ -270,14 +367,39 @@ NodeParameters::get_parameter(
 {
   std::lock_guard<std::mutex> lock(mutex_);
 
-  if (parameters_.count(name)) {
-    parameter = parameters_.at(name);
+  auto param_iter = parameters_.find(name);
+  if (
+    parameters_.end() != param_iter &&
+    param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET)
+  {
+    parameter = {name, param_iter->second.value};
     return true;
   } else {
     return false;
   }
 }
 
+bool
+NodeParameters::get_parameters_by_prefix(
+  const std::string & prefix,
+  std::map<std::string, rclcpp::Parameter> & parameters) const
+{
+  std::string prefix_with_dot = prefix + ".";
+  bool ret = false;
+
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  for (const auto & param : parameters_) {
+    if (param.first.find(prefix_with_dot) == 0 && param.first.length() > prefix_with_dot.length()) {
+      // Found one!
+      parameters[param.first.substr(prefix_with_dot.length())] = rclcpp::Parameter(param.second);
+      ret = true;
+    }
+  }
+
+  return ret;
+}
+
 std::vector<rcl_interfaces::msg::ParameterDescriptor>
 NodeParameters::describe_parameters(const std::vector<std::string> & names) const
 {
@@ -288,10 +410,7 @@ NodeParameters::describe_parameters(const std::vector<std::string> & names) cons
         return name == kv.first;
       }))
     {
-      rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
-      parameter_descriptor.name = kv.first;
-      parameter_descriptor.type = kv.second.get_type();
-      results.push_back(parameter_descriptor);
+      results.push_back(kv.second.descriptor);
     }
   }
   return results;
@@ -307,7 +426,7 @@ NodeParameters::get_parameter_types(const std::vector<std::string> & names) cons
         return name == kv.first;
       }))
     {
-      results.push_back(kv.second.get_type());
+      results.push_back(kv.second.value.get_type());
     } else {
       results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
     }

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_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/parameter.cpp

@@ -12,12 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "rclcpp/parameter.hpp"
+
 #include <ostream>
 #include <sstream>
 #include <string>
 #include <vector>
 
-#include "rclcpp/parameter.hpp"
+#include "rclcpp/node_interfaces/node_parameters.hpp"
 #include "rclcpp/utilities.hpp"
 
 using rclcpp::ParameterType;
@@ -33,6 +35,11 @@ Parameter::Parameter(const std::string & name, const rclcpp::ParameterValue & va
 {
 }
 
+Parameter::Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info)
+: Parameter(parameter_info.descriptor.name, parameter_info.value)
+{
+}
+
 ParameterType
 Parameter::get_type() const
 {
@@ -57,6 +64,12 @@ Parameter::get_value_message() const
   return value_.to_value_msg();
 }
 
+const rclcpp::ParameterValue &
+Parameter::get_parameter_value() const
+{
+  return value_;
+}
+
 bool
 Parameter::as_bool() const
 {

rclcpp/src/rclcpp/parameter_client.cpp

@@ -350,10 +350,60 @@ SyncParametersClient::SyncParametersClient(
   rclcpp::Node::SharedPtr node,
   const std::string & remote_node_name,
   const rmw_qos_profile_t & qos_profile)
-: executor_(executor), node_(node)
+: SyncParametersClient(
+    executor,
+    node->get_node_base_interface(),
+    node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface(),
+    remote_node_name,
+    qos_profile)
+{}
+
+SyncParametersClient::SyncParametersClient(
+  rclcpp::Node * node,
+  const std::string & remote_node_name,
+  const rmw_qos_profile_t & qos_profile)
+: SyncParametersClient(
+    std::make_shared<rclcpp::executors::SingleThreadedExecutor>(),
+    node,
+    remote_node_name,
+    qos_profile)
+{}
+
+SyncParametersClient::SyncParametersClient(
+  rclcpp::executor::Executor::SharedPtr executor,
+  rclcpp::Node * node,
+  const std::string & remote_node_name,
+  const rmw_qos_profile_t & qos_profile)
+: SyncParametersClient(
+    executor,
+    node->get_node_base_interface(),
+    node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface(),
+    remote_node_name,
+    qos_profile)
+{}
+
+SyncParametersClient::SyncParametersClient(
+  rclcpp::executor::Executor::SharedPtr executor,
+  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
+  const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface,
+  const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface,
+  const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
+  const std::string & remote_node_name,
+  const rmw_qos_profile_t & qos_profile)
+: executor_(executor), node_base_interface_(node_base_interface)
 {
   async_parameters_client_ =
-    std::make_shared<AsyncParametersClient>(node, remote_node_name, qos_profile);
+    std::make_shared<AsyncParametersClient>(
+    node_base_interface,
+    node_topics_interface,
+    node_graph_interface,
+    node_services_interface,
+    remote_node_name,
+    qos_profile);
 }
 
 std::vector<rclcpp::Parameter>
@@ -361,7 +411,7 @@ SyncParametersClient::get_parameters(const std::vector<std::string> & parameter_
 {
   auto f = async_parameters_client_->get_parameters(parameter_names);
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
+  if (spin_node_until_future_complete(*executor_, node_base_interface_, f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
@@ -385,7 +435,7 @@ SyncParametersClient::get_parameter_types(const std::vector<std::string> & param
   auto f = async_parameters_client_->get_parameter_types(parameter_names);
 
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
+  if (spin_node_until_future_complete(*executor_, node_base_interface_, f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
@@ -399,9 +449,8 @@ SyncParametersClient::set_parameters(
 {
   auto f = async_parameters_client_->set_parameters(parameters);
 
-  auto node_base_interface = node_->get_node_base_interface();
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_base_interface, f) ==
+  if (spin_node_until_future_complete(*executor_, node_base_interface_, f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
@@ -416,7 +465,7 @@ SyncParametersClient::set_parameters_atomically(
   auto f = async_parameters_client_->set_parameters_atomically(parameters);
 
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
+  if (spin_node_until_future_complete(*executor_, node_base_interface_, f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();
@@ -433,7 +482,7 @@ SyncParametersClient::list_parameters(
   auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
 
   using rclcpp::executors::spin_node_until_future_complete;
-  if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) ==
+  if (spin_node_until_future_complete(*executor_, node_base_interface_, f) ==
     rclcpp::executor::FutureReturnCode::SUCCESS)
   {
     return f.get();

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

@@ -41,7 +41,7 @@ SubscriptionBase::SubscriptionBase(
     {
       if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) {
         RCLCPP_ERROR(
-          rclcpp::get_logger(rcl_node_get_logger_name(node_handle.get())).get_child("rclcpp"),
+          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();

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,14 +33,16 @@ namespace rclcpp
 {
 
 TimeSource::TimeSource(std::shared_ptr<rclcpp::Node> node)
-: clock_subscription_(nullptr),
+: 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)
 {
 }
 
@@ -51,21 +52,49 @@ 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
 
+  logger_ = node_logging_->get_logger();
+
+  rclcpp::Parameter use_sim_time_param;
+  if (node_parameters_->get_parameter("use_sim_time", use_sim_time_param)) {
+    if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) {
+      if (use_sim_time_param.get_value<bool>() == true) {
+        parameter_state_ = SET_TRUE;
+        enable_ros_time();
+        create_clock_sub();
+      }
+    } else {
+      RCLCPP_ERROR(logger_, "Invalid type for parameter 'use_sim_time' %s should be bool",
+        use_sim_time_param.get_type_name().c_str());
+    }
+  } 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_,
@@ -86,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();
 }
 
@@ -97,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)
@@ -111,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();
   }
 }
@@ -200,7 +235,7 @@ 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) {
@@ -251,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,13 +18,22 @@
 #include <string>
 #include <memory>
 
+#include "rclcpp/contexts/default_context.hpp"
 #include "rcutils/logging_macros.h"
 
 using rclcpp::TimerBase;
 
-TimerBase::TimerBase(rclcpp::Clock::SharedPtr clock, std::chrono::nanoseconds period)
+TimerBase::TimerBase(
+  rclcpp::Clock::SharedPtr clock,
+  std::chrono::nanoseconds period,
+  rclcpp::Context::SharedPtr context)
+: clock_(clock), timer_handle_(nullptr)
 {
-  clock_ = clock;
+  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) mutable
@@ -36,15 +45,16 @@ TimerBase::TimerBase(rclcpp::Clock::SharedPtr clock, std::chrono::nanoseconds pe
         rcl_reset_error();
       }
       delete timer;
-      // Captured shared pointer by copy, reset to make sure timer is finalized before clock
+      // 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(), clock_handle, 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/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().str);
-      }
-    }
-  }
-  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().str;
-    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);
@@ -236,7 +97,7 @@ rclcpp::remove_ros_arguments(int argc, char const * const argv[])
         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,106 +113,62 @@ 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;
+  using rclcpp::contexts::default_context::get_global_default_context;
+  if (nullptr == context) {
+    context = get_global_default_context();
+  }
+  return context->is_valid();
 }
 
 bool
-rclcpp::is_initialized()
+rclcpp::is_initialized(rclcpp::Context::SharedPtr context)
 {
-  return rcl_ok();
-}
-
-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().str);
-      // *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().str);
-      // *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*
-  }
+  return rclcpp::ok(context);
 }
 
 bool
-rclcpp::sleep_for(const std::chrono::nanoseconds & nanoseconds)
+rclcpp::shutdown(rclcpp::Context::SharedPtr context, const std::string & reason)
 {
-  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;
+  using rclcpp::contexts::default_context::get_global_default_context;
+  auto default_context = get_global_default_context();
+  if (nullptr == context) {
+    context = default_context;
   }
-  if (time_left > std::chrono::nanoseconds::zero() && !g_is_interrupted) {
-    return sleep_for(time_left);
+  bool ret = context->shutdown(reason);
+  if (context == default_context) {
+    rclcpp::uninstall_signal_handlers();
   }
-  // Return true if the timeout elapsed successfully, otherwise false.
-  return !g_is_interrupted;
+  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/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_init.cpp

@@ -25,5 +25,5 @@ TEST(TestInit, is_initialized) {
 
   rclcpp::shutdown();
 
-  EXPECT_TRUE(rclcpp::is_initialized());
+  EXPECT_FALSE(rclcpp::is_initialized());
 }

rclcpp/test/test_local_parameters.cpp

@@ -0,0 +1,71 @@
+// 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 <cstdio>
+#include <map>
+#include <string>
+
+#include "gtest/gtest.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+TEST(test_local_parameters, set_parameter_if_not_set) {
+  auto node = rclcpp::Node::make_shared("test_local_parameters_set_parameter_if_not_set");
+
+  {
+    // try to set a map of parameters
+    std::map<std::string, double> bar_map{
+      {"x", 0.5},
+      {"y", 1.0},
+    };
+    node->set_parameters_if_not_set("bar", bar_map);
+    double bar_x_value;
+    ASSERT_TRUE(node->get_parameter("bar.x", bar_x_value));
+    EXPECT_EQ(bar_x_value, 0.5);
+    double bar_y_value;
+    ASSERT_TRUE(node->get_parameter("bar.y", bar_y_value));
+    EXPECT_EQ(bar_y_value, 1.0);
+    std::map<std::string, double> new_map;
+    ASSERT_TRUE(node->get_parameters("bar", new_map));
+    ASSERT_EQ(new_map.size(), 2U);
+    EXPECT_EQ(new_map["x"], 0.5);
+    EXPECT_EQ(new_map["y"], 1.0);
+  }
+
+  {
+    // try to get a map of parameters that doesn't exist
+    std::map<std::string, double> no_exist_map;
+    ASSERT_FALSE(node->get_parameters("no_exist", no_exist_map));
+  }
+
+  {
+    // set parameters for a map with different types, then try to get them back as a map
+    node->set_parameter_if_not_set("baz.x", 1.0);
+    node->set_parameter_if_not_set("baz.y", "hello");
+    std::map<std::string, double> baz_map;
+    EXPECT_THROW(node->get_parameters("baz", baz_map), rclcpp::ParameterTypeException);
+  }
+}
+
+int main(int argc, char ** argv)
+{
+  ::setvbuf(stdout, NULL, _IONBF, BUFSIZ);
+
+  // NOTE: use custom main to ensure that rclcpp::init is called only once
+  rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return ret;
+}

rclcpp/test/test_node_initial_parameters.cpp

@@ -44,10 +44,11 @@ TEST_F(TestNodeWithInitialValues, no_initial_values) {
   auto node = rclcpp::Node::make_shared(
     "node_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process);
   auto list_params_result = node->list_parameters({}, 0);
-  EXPECT_EQ(0u, list_params_result.names.size());
+  // Has use_sim_time parameter
+  EXPECT_EQ(1u, list_params_result.names.size());
 }
 
-TEST_F(TestNodeWithInitialValues, multiple_initial_values) {
+TEST_F(TestNodeWithInitialValues, multiple_undeclared_initial_values) {
   auto context = rclcpp::contexts::default_context::get_global_default_context();
   const std::vector<std::string> arguments = {};
   const std::vector<rclcpp::Parameter> initial_values = {
@@ -60,7 +61,53 @@ TEST_F(TestNodeWithInitialValues, multiple_initial_values) {
   auto node = rclcpp::Node::make_shared(
     "node_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process);
   auto list_params_result = node->list_parameters({}, 0);
-  EXPECT_EQ(3u, list_params_result.names.size());
+  // Has use_sim_time parameter
+  EXPECT_EQ(1u, list_params_result.names.size());
+}
+
+TEST_F(TestNodeWithInitialValues, multiple_declared_initial_values) {
+  auto context = rclcpp::contexts::default_context::get_global_default_context();
+  const std::vector<std::string> arguments = {};
+  const std::vector<rclcpp::Parameter> initial_values = {
+    rclcpp::Parameter("foo", true),
+    rclcpp::Parameter("bar", "hello world"),
+    rclcpp::Parameter("baz", std::vector<double>{3.14, 2.718})
+  };
+  const bool use_global_arguments = false;
+  const bool use_intra_process = false;
+  auto node = rclcpp::Node::make_shared(
+    "node_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process);
+
+  node->declare_parameter("foo");
+  node->declare_parameter("bar");
+  node->declare_parameter("baz");
+
+  auto list_params_result = node->list_parameters({}, 0);
+  EXPECT_TRUE(node->get_parameter("foo").get_value<bool>());
+  EXPECT_STREQ("hello world", node->get_parameter("bar").get_value<std::string>().c_str());
+  std::vector<double> double_array = node->get_parameter("baz").get_value<std::vector<double>>();
+  ASSERT_EQ(2u, double_array.size());
+  EXPECT_DOUBLE_EQ(3.14, double_array.at(0));
+  EXPECT_DOUBLE_EQ(2.718, double_array.at(1));
+}
+
+TEST_F(TestNodeWithInitialValues, multiple_undeclared_initial_values_allowed) {
+  auto context = rclcpp::contexts::default_context::get_global_default_context();
+  const std::vector<std::string> arguments = {};
+  const std::vector<rclcpp::Parameter> initial_values = {
+    rclcpp::Parameter("foo", true),
+    rclcpp::Parameter("bar", "hello world"),
+    rclcpp::Parameter("baz", std::vector<double>{3.14, 2.718})
+  };
+  const bool use_global_arguments = false;
+  const bool use_intra_process = false;
+  const bool start_param_services = true;
+  const bool allow_undeclared_params = true;
+  auto node = rclcpp::Node::make_shared(
+    "node_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process,
+    start_param_services, allow_undeclared_params);
+
+  auto list_params_result = node->list_parameters({}, 0);
   EXPECT_TRUE(node->get_parameter("foo").get_value<bool>());
   EXPECT_STREQ("hello world", node->get_parameter("bar").get_value<std::string>().c_str());
   std::vector<double> double_array = node->get_parameter("baz").get_value<std::vector<double>>();

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");
@@ -107,7 +112,7 @@ TEST_F(TestTimeSource, reattach) {
   ASSERT_NO_THROW(ts.attachNode(node));
 }
 
-TEST_F(TestTimeSource, ROS_time_valid) {
+TEST_F(TestTimeSource, ROS_time_valid_attach_detach) {
   rclcpp::TimeSource ts;
   auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
 
@@ -126,6 +131,37 @@ TEST_F(TestTimeSource, ROS_time_valid) {
   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);
@@ -348,14 +384,14 @@ TEST_F(TestTimeSource, parameter_activation) {
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
   set_use_sim_time_parameter(
-    node, rclcpp::ParameterValue(rclcpp::ParameterType::PARAMETER_NOT_SET));
+    node, rclcpp::ParameterValue());
   EXPECT_TRUE(ros_clock->ros_time_is_active());
 
   set_use_sim_time_parameter(node, rclcpp::ParameterValue(false));
   EXPECT_FALSE(ros_clock->ros_time_is_active());
 
   set_use_sim_time_parameter(
-    node, rclcpp::ParameterValue(rclcpp::ParameterType::PARAMETER_NOT_SET));
+    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

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"
 
@@ -49,5 +50,34 @@ TEST(TestUtilities, init_with_args) {
   ASSERT_EQ(1u, other_args.size());
   ASSERT_EQ(std::string{"process_name"}, other_args[0]);
 
-  EXPECT_TRUE(rclcpp::is_initialized());
+  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_

rclcpp_action/include/rclcpp_action/create_server.hpp

@@ -0,0 +1,89 @@
+// 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_SERVER_HPP_
+#define RCLCPP_ACTION__CREATE_SERVER_HPP_
+
+#include <rcl_action/action_server.h>
+
+#include <rclcpp/node.hpp>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_clock_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
+#include <rclcpp/node_interfaces/node_waitables_interface.hpp>
+
+#include <memory>
+#include <string>
+
+#include "rclcpp_action/server.hpp"
+#include "rclcpp_action/visibility_control.hpp"
+
+namespace rclcpp_action
+{
+template<typename ActionT>
+typename Server<ActionT>::SharedPtr
+create_server(
+  rclcpp::Node::SharedPtr node,
+  const std::string & name,
+  typename Server<ActionT>::GoalCallback handle_goal,
+  typename Server<ActionT>::CancelCallback handle_cancel,
+  typename Server<ActionT>::AcceptedCallback handle_accepted,
+  const rcl_action_server_options_t & options = rcl_action_server_get_default_options(),
+  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
+{
+  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](Server<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<Server<ActionT>> fake_shared_ptr(ptr, [](Server<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<Server<ActionT>> action_server(new Server<ActionT>(
+      node->get_node_base_interface(),
+      node->get_node_clock_interface(),
+      node->get_node_logging_interface(),
+      name,
+      options,
+      handle_goal,
+      handle_cancel,
+      handle_accepted), deleter);
+
+  node->get_node_waitables_interface()->add_waitable(action_server, group);
+  return action_server;
+}
+}  // namespace rclcpp_action
+#endif  // RCLCPP_ACTION__CREATE_SERVER_HPP_

rclcpp_action/include/rclcpp_action/exceptions.hpp

@@ -0,0 +1,46 @@
+// 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__EXCEPTIONS_HPP_
+#define RCLCPP_ACTION__EXCEPTIONS_HPP_
+
+#include <stdexcept>
+
+namespace rclcpp_action
+{
+namespace exceptions
+{
+class UnknownGoalHandleError : public std::invalid_argument
+{
+public:
+  UnknownGoalHandleError()
+  : std::invalid_argument("Goal handle is not know to this client.")
+  {
+  }
+};
+
+class UnawareGoalHandleError : public std::runtime_error
+{
+public:
+  UnawareGoalHandleError()
+  : std::runtime_error("Goal handle is not tracking the goal result.")
+  {
+  }
+};
+
+}  // namespace exceptions
+
+}  // namespace rclcpp_action
+
+#endif  // RCLCPP_ACTION__EXCEPTIONS_HPP_

rclcpp_action/include/rclcpp_action/rclcpp_action.hpp

@@ -0,0 +1,39 @@
+// 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.
+
+/** \mainpage rclcpp_action: ROS Action Client Library for C++
+ *
+ * `rclcpp_action` provides the canonical C++ API for interacting with ROS Actions.
+ * It consists of these main components:
+ *
+ * - TODO(jacobperron): Finish docs
+ * - Action Client
+ * - Action Server
+ */
+
+#ifndef RCLCPP_ACTION__RCLCPP_ACTION_HPP_
+#define RCLCPP_ACTION__RCLCPP_ACTION_HPP_
+
+#include <csignal>
+#include <memory>
+
+#include "rclcpp_action/client.hpp"
+#include "rclcpp_action/client_goal_handle.hpp"
+#include "rclcpp_action/create_client.hpp"
+#include "rclcpp_action/create_server.hpp"
+#include "rclcpp_action/server.hpp"
+#include "rclcpp_action/server_goal_handle.hpp"
+#include "rclcpp_action/visibility_control.hpp"
+
+#endif  // RCLCPP_ACTION__RCLCPP_ACTION_HPP_

rclcpp_action/include/rclcpp_action/server.hpp

@@ -0,0 +1,487 @@
+// 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__SERVER_HPP_
+#define RCLCPP_ACTION__SERVER_HPP_
+
+#include <rcl_action/action_server.h>
+#include <rosidl_generator_c/action_type_support_struct.h>
+#include <rosidl_typesupport_cpp/action_type_support.hpp>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_clock_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
+#include <rclcpp/waitable.hpp>
+
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <utility>
+
+#include "rclcpp_action/visibility_control.hpp"
+#include "rclcpp_action/server_goal_handle.hpp"
+#include "rclcpp_action/types.hpp"
+
+namespace rclcpp_action
+{
+// Forward declaration
+class ServerBaseImpl;
+
+/// A response returned by an action server callback when a goal is requested.
+enum class GoalResponse : int8_t
+{
+  /// The goal is rejected and will not be executed.
+  REJECT = 1,
+  /// The server accepts the goal, and is going to begin execution immediately.
+  ACCEPT_AND_EXECUTE = 2,
+  /// The server accepts the goal, and is going to execute it later.
+  ACCEPT_AND_DEFER = 3,
+};
+
+/// A response returned by an action server callback when a goal has been asked to be canceled.
+enum class CancelResponse : int8_t
+{
+  /// The server will not try to cancel the goal.
+  REJECT = 1,
+  /// The server has agreed to try to cancel the goal.
+  ACCEPT = 2,
+};
+
+/// Base Action Server implementation
+/// \internal
+/**
+ * This class should not be used directly by users writing an action server.
+ * Instead users should use `rclcpp_action::Server<>`.
+ *
+ * Internally, this class is responsible for interfacing with the `rcl_action` API.
+ */
+class ServerBase : public rclcpp::Waitable
+{
+public:
+  RCLCPP_ACTION_PUBLIC
+  virtual ~ServerBase();
+
+  // -------------
+  // Waitables API
+
+  /// Return the number of subscriptions used to implement an action server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_subscriptions() override;
+
+  /// Return the number of timers used to implement an action server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_timers() override;
+
+  /// Return the number of service clients used to implement an action server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_clients() override;
+
+  /// Return the number of service servers used to implement an action server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_services() override;
+
+  /// Return the number of guard conditions used to implement an action server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_guard_conditions() override;
+
+  /// Add all entities to a wait set.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  /// Return true if any entity belonging to the action server is ready to be executed.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t *) override;
+
+  /// Act on entities in the wait set which are ready to be acted upon.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute() override;
+
+  // End Waitables API
+  // -----------------
+
+protected:
+  RCLCPP_ACTION_PUBLIC
+  ServerBase(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    const std::string & name,
+    const rosidl_action_type_support_t * type_support,
+    const rcl_action_server_options_t & options);
+
+  // -----------------------------------------------------
+  // API for communication between ServerBase and Server<>
+
+  // ServerBase will call this function when a goal request is received.
+  // The subclass should convert to the real type and call a user's callback.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  std::pair<GoalResponse, std::shared_ptr<void>>
+  call_handle_goal_callback(GoalID &, std::shared_ptr<void> request) = 0;
+
+  // ServerBase will determine which goal ids are being cancelled, and then call this function for
+  // each goal id.
+  // The subclass should look up a goal handle and call the user's callback.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  CancelResponse
+  call_handle_cancel_callback(const GoalID & uuid) = 0;
+
+  /// Given a goal request message, return the UUID contained within.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  GoalID
+  get_goal_id_from_goal_request(void * message) = 0;
+
+  /// Create an empty goal request message so it can be taken from a lower layer.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  std::shared_ptr<void>
+  create_goal_request() = 0;
+
+  /// Call user callback to inform them a goal has been accepted.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  call_goal_accepted_callback(
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
+    GoalID uuid, std::shared_ptr<void> goal_request_message) = 0;
+
+  /// Given a result request message, return the UUID contained within.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  GoalID
+  get_goal_id_from_result_request(void * message) = 0;
+
+  /// Create an empty goal request message so it can be taken from a lower layer.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  std::shared_ptr<void>
+  create_result_request() = 0;
+
+  /// Create an empty goal result message so it can be sent as a reply in a lower layer
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  std::shared_ptr<void>
+  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  publish_status();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  notify_goal_terminal_state();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg);
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  publish_feedback(std::shared_ptr<void> feedback_msg);
+
+  // End API for communication between ServerBase and Server<>
+  // ---------------------------------------------------------
+
+private:
+  /// Handle a request to add a new goal to the server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute_goal_request_received();
+
+  /// Handle a request to cancel goals on the server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute_cancel_request_received();
+
+  /// Handle a request to get the result of an action
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute_result_request_received();
+
+  /// Handle a timeout indicating a completed goal should be forgotten by the server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute_check_expired_goals();
+
+  /// Private implementation
+  /// \internal
+  std::unique_ptr<ServerBaseImpl> pimpl_;
+};
+
+/// Action Server
+/**
+ * This class creates an action server.
+ *
+ * Create an instance of this server using `rclcpp_action::create_server()`.
+ *
+ * Internally, this class is responsible for:
+ *  - coverting between the C++ action type and generic types for `rclcpp_action::ServerBase`, and
+ *  - calling user callbacks.
+ */
+template<typename ActionT>
+class Server : public ServerBase, public std::enable_shared_from_this<Server<ActionT>>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server)
+
+  /// Signature of a callback that accepts or rejects goal requests.
+  using GoalCallback = std::function<GoalResponse(
+        const GoalID &, std::shared_ptr<const typename ActionT::Goal>)>;
+  /// Signature of a callback that accepts or rejects requests to cancel a goal.
+  using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
+  /// Signature of a callback that is used to notify when the goal has been accepted.
+  using AcceptedCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;
+
+  /// Construct an action server.
+  /**
+   * This constructs an action server, but it will not work until it has been added to a node.
+   * Use `rclcpp_action::create_server()` to both construct and add to a node.
+   *
+   * Three callbacks must be provided:
+   *  - one to accept or reject goals sent to the server,
+   *  - one to accept or reject requests to cancel a goal,
+   *  - one to receive a goal handle after a goal has been accepted.
+   * All callbacks must be non-blocking.
+   * The result of a goal should be set using methods on `rclcpp_action::ServerGoalHandle<>`.
+   *
+   * \param[in] node_base a pointer to the base interface of a node.
+   * \param[in] node_clock a pointer to an interface that allows getting a node's clock.
+   * \param[in] node_logging a pointer to an interface that allows getting a node's logger.
+   * \param[in] name the name of an action.
+   *  The same name and type must be used by both the action client and action server to
+   *  communicate.
+   * \param[in] options options to pass to the underlying `rcl_action_server_t`.
+   * \param[in] handle_goal a callback that decides if a goal should be accepted or rejected.
+   * \param[in] handle_cancel a callback that decides if a goal should be attemted to be canceled.
+   *  The return from this callback only indicates if the server will try to cancel a goal.
+   *  It does not indicate if the goal was actually canceled.
+   * \param[in] handle_accepted a callback that is called to give the user a handle to the goal.
+   *  execution.
+   */
+  Server(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    const std::string & name,
+    const rcl_action_server_options_t & options,
+    GoalCallback handle_goal,
+    CancelCallback handle_cancel,
+    AcceptedCallback handle_accepted
+  )
+  : ServerBase(
+      node_base,
+      node_clock,
+      node_logging,
+      name,
+      rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
+      options),
+    handle_goal_(handle_goal),
+    handle_cancel_(handle_cancel),
+    handle_accepted_(handle_accepted)
+  {
+  }
+
+  virtual ~Server() = default;
+
+protected:
+  // -----------------------------------------------------
+  // API for communication between ServerBase and Server<>
+
+  /// \internal
+  std::pair<GoalResponse, std::shared_ptr<void>>
+  call_handle_goal_callback(GoalID & uuid, std::shared_ptr<void> message) override
+  {
+    // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type
+    static_assert(
+      std::is_same<typename ActionT::Goal, typename ActionT::GoalRequestService::Request>::value,
+      "Assuming user fields were merged with goal request fields");
+    GoalResponse user_response = handle_goal_(
+      uuid, std::static_pointer_cast<typename ActionT::Goal>(message));
+
+    auto ros_response = std::make_shared<typename ActionT::GoalRequestService::Response>();
+    ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response ||
+      GoalResponse::ACCEPT_AND_DEFER == user_response;
+    return std::make_pair(user_response, ros_response);
+  }
+
+  /// \internal
+  CancelResponse
+  call_handle_cancel_callback(const GoalID & uuid) override
+  {
+    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    CancelResponse resp = CancelResponse::REJECT;
+    auto element = goal_handles_.find(uuid);
+    if (element != goal_handles_.end()) {
+      std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle = element->second.lock();
+      if (goal_handle) {
+        resp = handle_cancel_(goal_handle);
+        if (CancelResponse::ACCEPT == resp) {
+          goal_handle->_set_canceling();
+        }
+      }
+    }
+    return resp;
+  }
+
+  /// \internal
+  void
+  call_goal_accepted_callback(
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
+    GoalID uuid, std::shared_ptr<void> goal_request_message) override
+  {
+    std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
+    std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();
+
+    std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state =
+      [weak_this](const GoalID & uuid, std::shared_ptr<void> result_message)
+      {
+        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
+        if (!shared_this) {
+          return;
+        }
+        // Send result message to anyone that asked
+        shared_this->publish_result(uuid, result_message);
+        // Publish a status message any time a goal handle changes state
+        shared_this->publish_status();
+        // notify base so it can recalculate the expired goal timer
+        shared_this->notify_goal_terminal_state();
+        // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires)
+        std::lock_guard<std::mutex> lock(shared_this->goal_handles_mutex_);
+        shared_this->goal_handles_.erase(uuid);
+      };
+
+    std::function<void(const GoalID &)> on_executing =
+      [weak_this](const GoalID & uuid)
+      {
+        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
+        if (!shared_this) {
+          return;
+        }
+        (void)uuid;
+        // Publish a status message any time a goal handle changes state
+        shared_this->publish_status();
+      };
+
+    std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback =
+      [weak_this](std::shared_ptr<typename ActionT::Feedback> feedback_msg)
+      {
+        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
+        if (!shared_this) {
+          return;
+        }
+        shared_this->publish_feedback(std::static_pointer_cast<void>(feedback_msg));
+      };
+
+    goal_handle.reset(
+      new ServerGoalHandle<ActionT>(
+        rcl_goal_handle, uuid,
+        std::static_pointer_cast<const typename ActionT::Goal>(goal_request_message),
+        on_terminal_state, on_executing, publish_feedback));
+    {
+      std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+      goal_handles_[uuid] = goal_handle;
+    }
+    handle_accepted_(goal_handle);
+  }
+
+  /// \internal
+  GoalID
+  get_goal_id_from_goal_request(void * message) override
+  {
+    return
+      static_cast<typename ActionT::GoalRequestService::Request *>(message)->action_goal_id.uuid;
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_goal_request() override
+  {
+    return std::shared_ptr<void>(new typename ActionT::GoalRequestService::Request());
+  }
+
+  /// \internal
+  GoalID
+  get_goal_id_from_result_request(void * message) override
+  {
+    return
+      static_cast<typename ActionT::GoalResultService::Request *>(message)->action_goal_id.uuid;
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_result_request() override
+  {
+    return std::shared_ptr<void>(new typename ActionT::GoalResultService::Request());
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
+  {
+    auto result = std::make_shared<typename ActionT::GoalResultService::Response>();
+    result->action_status = status;
+    return std::static_pointer_cast<void>(result);
+  }
+
+  // End API for communication between ServerBase and Server<>
+  // ---------------------------------------------------------
+
+private:
+  GoalCallback handle_goal_;
+  CancelCallback handle_cancel_;
+  AcceptedCallback handle_accepted_;
+
+  using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>;
+  /// A map of goal id to goal handle weak pointers.
+  /// This is used to provide a goal handle to handle_cancel.
+  std::unordered_map<GoalID, GoalHandleWeakPtr> goal_handles_;
+  std::mutex goal_handles_mutex_;
+};
+}  // namespace rclcpp_action
+#endif  // RCLCPP_ACTION__SERVER_HPP_

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp

@@ -0,0 +1,272 @@
+// 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__SERVER_GOAL_HANDLE_HPP_
+#define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_
+
+#include <rcl_action/types.h>
+#include <rcl_action/goal_handle.h>
+
+#include <action_msgs/msg/goal_status.hpp>
+
+#include <functional>
+#include <memory>
+#include <mutex>
+
+#include "rclcpp_action/visibility_control.hpp"
+#include "rclcpp_action/types.hpp"
+
+namespace rclcpp_action
+{
+
+/// Base class to interact with goals on a server.
+/// \internal
+/**
+ *
+ * This class in not be used directly by users writing an action server.
+ * Instead users will be given an instance of `rclcpp_action::ServerGoalHandle<>`.
+ *
+ * Internally, this class is responsible for interfacing with the `rcl_action` API.
+ */
+class ServerGoalHandleBase
+{
+public:
+  /// Indicate if client has requested this goal be cancelled.
+  /// \return true if a cancelation request has been accepted for this goal.
+  RCLCPP_ACTION_PUBLIC
+  bool
+  is_canceling() const;
+
+  /// Indicate if goal is pending or executing.
+  /// \return false if goal has reached a terminal state.
+  RCLCPP_ACTION_PUBLIC
+  bool
+  is_active() const;
+
+  /// Indicate if goal is executing.
+  /// \return true only if the goal is in an executing state.
+  RCLCPP_ACTION_PUBLIC
+  bool
+  is_executing() const;
+
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  ~ServerGoalHandleBase();
+
+protected:
+  // -------------------------------------------------------------------------
+  // API for communication between ServerGoalHandleBase and ServerGoalHandle<>
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  ServerGoalHandleBase(
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle
+  )
+  : rcl_handle_(rcl_handle)
+  {
+  }
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  _set_aborted();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  _set_succeeded();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  _set_canceling();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  _set_canceled();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  _set_executing();
+
+  /// Transition the goal to canceled state if it never reached a terminal state.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  try_canceling() noexcept;
+
+  // End API for communication between ServerGoalHandleBase and ServerGoalHandle<>
+  // -----------------------------------------------------------------------------
+
+private:
+  std::shared_ptr<rcl_action_goal_handle_t> rcl_handle_;
+  mutable std::mutex rcl_handle_mutex_;
+};
+
+// Forward declare server
+template<typename ActionT>
+class Server;
+
+/// Class to interact with goals on a server.
+/**
+ * Use this class to check the status of a goal as well as set the result.
+ * This class is not meant to be created by a user, instead it is created when a goal has been
+ * accepted.
+ * The class `rclcpp_action::Server<>` will create an instance and give it to the user in their
+ * `handle_accepted` callback.
+ *
+ * Internally, this class is responsible for coverting between the C++ action type and generic
+ * types for `rclcpp_action::ServerGoalHandleBase`.
+ */
+template<typename ActionT>
+class ServerGoalHandle : public ServerGoalHandleBase
+{
+public:
+  /// Send an update about the progress of a goal.
+  /**
+   * This must only be called when the goal is executing.
+   * If execution of a goal is deferred then `ServerGoalHandle<>::set_executing()` must be called
+   * first.
+   * `std::runtime_error` is raised if the goal is in any state besides executing.
+   *
+   * \param[in] feedback_msg the message to publish to clients.
+   */
+  void
+  publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
+  {
+    feedback_msg->action_goal_id.uuid = uuid_;
+    publish_feedback_(feedback_msg);
+  }
+
+  // TODO(sloretz) which exception is raised?
+  /// Indicate that a goal could not be reached and has been aborted.
+  /**
+   * Only call this if the goal was executing but cannot be completed.
+   * This is a terminal state, no more methods should be called on a goal handle after this is
+   * called.
+   * An exception is raised if the goal is in any state besides executing.
+   *
+   * \param[in] result_msg the final result to send to clients.
+   */
+  void
+  set_aborted(typename ActionT::Result::SharedPtr result_msg)
+  {
+    _set_aborted();
+    result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
+    on_terminal_state_(uuid_, result_msg);
+  }
+
+  /// Indicate that a goal has been reached.
+  /**
+   * Only call this if the goal is executing and has reached the desired final state.
+   * This is a terminal state, no more methods should be called on a goal handle after this is
+   * called.
+   * An exception is raised if the goal is in any state besides executing.
+   *
+   * \param[in] result_msg the final result to send to clients.
+   */
+  void
+  set_succeeded(typename ActionT::Result::SharedPtr result_msg)
+  {
+    _set_succeeded();
+    result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
+    on_terminal_state_(uuid_, result_msg);
+  }
+
+  /// Indicate that a goal has been canceled.
+  /**
+   * Only call this if the goal is executing or pending, but has been canceled.
+   * This is a terminal state, no more methods should be called on a goal handle after this is
+   * called.
+   * An exception is raised if the goal is in any state besides executing or pending.
+   *
+   * \param[in] result_msg the final result to send to clients.
+   */
+  void
+  set_canceled(typename ActionT::Result::SharedPtr result_msg)
+  {
+    _set_canceled();
+    result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
+    on_terminal_state_(uuid_, result_msg);
+  }
+
+  /// Indicate that the server is starting to execute a goal.
+  /**
+   * Only call this if the goal is pending.
+   * An exception is raised if the goal is in any state besides pending.
+   */
+  void
+  set_executing()
+  {
+    _set_executing();
+    on_executing_(uuid_);
+  }
+
+  /// Get the original request message describing the goal.
+  const std::shared_ptr<const typename ActionT::Goal>
+  get_goal() const
+  {
+    return goal_;
+  }
+
+  /// Get the unique identifier of the goal
+  const GoalID &
+  get_goal_id() const
+  {
+    return uuid_;
+  }
+
+  virtual ~ServerGoalHandle()
+  {
+    // Cancel goal if handle was allowed to destruct without reaching a terminal state
+    if (try_canceling()) {
+      auto null_result = std::make_shared<typename ActionT::Result>();
+      null_result->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
+      on_terminal_state_(uuid_, null_result);
+    }
+  }
+
+protected:
+  /// \internal
+  ServerGoalHandle(
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
+    GoalID uuid,
+    std::shared_ptr<const typename ActionT::Goal> goal,
+    std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state,
+    std::function<void(const GoalID &)> on_executing,
+    std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback
+  )
+  : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid),
+    on_terminal_state_(on_terminal_state), on_executing_(on_executing),
+    publish_feedback_(publish_feedback)
+  {
+  }
+
+  /// The original request message describing the goal.
+  const std::shared_ptr<const typename ActionT::Goal> goal_;
+
+  /// A unique id for the goal request.
+  const GoalID uuid_;
+
+  friend Server<ActionT>;
+
+  std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state_;
+  std::function<void(const GoalID &)> on_executing_;
+  std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback_;
+};
+}  // namespace rclcpp_action
+
+#endif  // RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_

rclcpp_action/include/rclcpp_action/types.hpp

@@ -0,0 +1,84 @@
+// 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__TYPES_HPP_
+#define RCLCPP_ACTION__TYPES_HPP_
+
+#include <rcl_action/types.h>
+
+#include <action_msgs/msg/goal_status.hpp>
+#include <action_msgs/msg/goal_info.hpp>
+
+#include <climits>
+#include <functional>
+#include <string>
+
+#include "rclcpp_action/visibility_control.hpp"
+
+namespace rclcpp_action
+{
+
+using GoalID = std::array<uint8_t, UUID_SIZE>;
+using GoalStatus = action_msgs::msg::GoalStatus;
+using GoalInfo = action_msgs::msg::GoalInfo;
+
+/// Convert a goal id to a human readable string.
+RCLCPP_ACTION_PUBLIC
+std::string
+to_string(const GoalID & goal_id);
+
+// Convert C++ GoalID to rcl_action_goal_info_t
+RCLCPP_ACTION_PUBLIC
+void
+convert(const GoalID & goal_id, rcl_action_goal_info_t * info);
+
+// Convert rcl_action_goal_info_t to C++ GoalID
+RCLCPP_ACTION_PUBLIC
+void
+convert(const rcl_action_goal_info_t & info, GoalID * goal_id);
+}  // namespace rclcpp_action
+
+namespace std
+{
+template<>
+struct less<rclcpp_action::GoalID>
+{
+  bool operator()(
+    const rclcpp_action::GoalID & lhs,
+    const rclcpp_action::GoalID & rhs) const
+  {
+    return lhs < rhs;
+  }
+};
+
+/// Hash a goal id so it can be used as a key in std::unordered_map
+template<>
+struct hash<rclcpp_action::GoalID>
+{
+  size_t operator()(const rclcpp_action::GoalID & uuid) const noexcept
+  {
+    // TODO(sloretz) Use someone else's hash function and cite it
+    size_t result = 0;
+    for (size_t i = 0; i < uuid.size(); ++i) {
+      for (size_t b = 0; b < sizeof(size_t); ++b) {
+        size_t part = uuid[i];
+        part <<= CHAR_BIT * b;
+        result ^= part;
+      }
+    }
+    return result;
+  }
+};
+}  // namespace std
+#endif  // RCLCPP_ACTION__TYPES_HPP_

rclcpp_action/include/rclcpp_action/visibility_control.hpp

@@ -0,0 +1,56 @@
+// 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.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef RCLCPP_ACTION__VISIBILITY_CONTROL_HPP_
+#define RCLCPP_ACTION__VISIBILITY_CONTROL_HPP_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define RCLCPP_ACTION_EXPORT __attribute__ ((dllexport))
+    #define RCLCPP_ACTION_IMPORT __attribute__ ((dllimport))
+  #else
+    #define RCLCPP_ACTION_EXPORT __declspec(dllexport)
+    #define RCLCPP_ACTION_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef RCLCPP_ACTION_BUILDING_LIBRARY
+    #define RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_EXPORT
+  #else
+    #define RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_IMPORT
+  #endif
+  #define RCLCPP_ACTION_PUBLIC_TYPE RCLCPP_ACTION_PUBLIC
+  #define RCLCPP_ACTION_LOCAL
+#else
+  #define RCLCPP_ACTION_EXPORT __attribute__ ((visibility("default")))
+  #define RCLCPP_ACTION_IMPORT
+  #if __GNUC__ >= 4
+    #define RCLCPP_ACTION_PUBLIC __attribute__ ((visibility("default")))
+    #define RCLCPP_ACTION_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define RCLCPP_ACTION_PUBLIC
+    #define RCLCPP_ACTION_LOCAL
+  #endif
+  #define RCLCPP_ACTION_PUBLIC_TYPE
+#endif
+
+#endif  // RCLCPP_ACTION__VISIBILITY_CONTROL_HPP_

rclcpp_action/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>rclcpp_action</name>
+  <version>0.6.2</version>
+  <description>Adds action APIs for C++.</description>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <build_export_depend>rosidl_generator_cpp</build_export_depend>
+  <build_export_depend>rosidl_generator_c</build_export_depend>
+
+  <build_depend>rosidl_generator_c</build_depend>
+  <build_depend>rosidl_generator_cpp</build_depend>
+
+  <depend>action_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rcl_action</depend>
+
+  <depend>ament_cmake</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>test_msgs</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

rclcpp_action/src/client.cpp

@@ -0,0 +1,444 @@
+// 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 <rcl_action/action_client.h>
+#include <rcl_action/wait.h>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
+
+#include <algorithm>
+#include <map>
+#include <memory>
+#include <random>
+#include <string>
+
+#include "rclcpp_action/client.hpp"
+#include "rclcpp_action/exceptions.hpp"
+
+namespace rclcpp_action
+{
+
+class ClientBaseImpl
+{
+public:
+  ClientBaseImpl(
+    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 & client_options)
+  : node_graph_(node_graph),
+    node_handle(node_base->get_shared_rcl_node_handle()),
+    logger(node_logging->get_logger().get_child("rclcpp_acton")),
+    random_bytes_generator(std::random_device{} ())
+  {
+    std::weak_ptr<rcl_node_t> weak_node_handle(node_handle);
+    client_handle = std::shared_ptr<rcl_action_client_t>(
+      new rcl_action_client_t, [weak_node_handle](rcl_action_client_t * client)
+      {
+        auto handle = weak_node_handle.lock();
+        if (handle) {
+          if (RCL_RET_OK != rcl_action_client_fini(client, handle.get())) {
+            RCLCPP_ERROR(
+              rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp_action"),
+              "Error in destruction of rcl action client handle: %s", rcl_get_error_string().str);
+            rcl_reset_error();
+          }
+        } else {
+          RCLCPP_ERROR(
+            rclcpp::get_logger("rclcpp_action"),
+            "Error in destruction of rcl action client handle: "
+            "the Node Handle was destructed too early. You will leak memory");
+        }
+        delete client;
+      });
+    *client_handle = rcl_action_get_zero_initialized_client();
+    rcl_ret_t ret = rcl_action_client_init(
+      client_handle.get(), node_handle.get(), type_support,
+      action_name.c_str(), &client_options);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "could not initialize rcl action client");
+    }
+
+    ret = rcl_action_client_wait_set_get_num_entities(
+      client_handle.get(),
+      &num_subscriptions,
+      &num_guard_conditions,
+      &num_timers,
+      &num_clients,
+      &num_services);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "could not retrieve rcl action client details");
+    }
+  }
+
+  size_t num_subscriptions{0u};
+  size_t num_guard_conditions{0u};
+  size_t num_timers{0u};
+  size_t num_clients{0u};
+  size_t num_services{0u};
+
+  bool is_feedback_ready{false};
+  bool is_status_ready{false};
+  bool is_goal_response_ready{false};
+  bool is_cancel_response_ready{false};
+  bool is_result_response_ready{false};
+
+  rclcpp::Context::SharedPtr context_;
+  rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
+  std::shared_ptr<rcl_action_client_t> client_handle{nullptr};
+  std::shared_ptr<rcl_node_t> node_handle{nullptr};
+  rclcpp::Logger logger;
+
+  using ResponseCallback = std::function<void (std::shared_ptr<void> response)>;
+
+  std::map<int64_t, ResponseCallback> pending_goal_responses;
+  std::mutex goal_requests_mutex;
+
+  std::map<int64_t, ResponseCallback> pending_result_responses;
+  std::mutex result_requests_mutex;
+
+  std::map<int64_t, ResponseCallback> pending_cancel_responses;
+  std::mutex cancel_requests_mutex;
+
+  std::independent_bits_engine<
+    std::default_random_engine, 8, unsigned int> random_bytes_generator;
+};
+
+ClientBase::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 & client_options)
+: pimpl_(new ClientBaseImpl(
+      node_base, node_graph, node_logging, action_name, type_support, client_options))
+{
+}
+
+ClientBase::~ClientBase()
+{
+}
+
+bool
+ClientBase::action_server_is_ready() const
+{
+  bool is_ready;
+  rcl_ret_t ret = rcl_action_server_is_available(
+    this->pimpl_->node_handle.get(),
+    this->pimpl_->client_handle.get(),
+    &is_ready);
+  if (RCL_RET_NODE_INVALID == ret) {
+    const rcl_node_t * node_handle = this->pimpl_->node_handle.get();
+    if (node_handle && !rcl_context_is_valid(node_handle->context)) {
+      // context is shutdown, do a soft failure
+      return false;
+    }
+  }
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_action_server_is_available failed");
+  }
+  return is_ready;
+}
+
+bool
+ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout)
+{
+  auto start = std::chrono::steady_clock::now();
+  // make an event to reuse, rather than create a new one each time
+  auto node_ptr = pimpl_->node_graph_.lock();
+  if (!node_ptr) {
+    throw rclcpp::exceptions::InvalidNodeError();
+  }
+  auto event = node_ptr->get_graph_event();
+  // check to see if the server is ready immediately
+  if (this->action_server_is_ready()) {
+    return true;
+  }
+  if (timeout == std::chrono::nanoseconds(0)) {
+    // check was non-blocking, return immediately
+    return false;
+  }
+  // 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::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);
+  }
+  do {
+    if (!rclcpp::ok(this->pimpl_->context_)) {
+      return false;
+    }
+    // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation.
+    // A race condition means that graph changes for services becoming available may trigger the
+    // wait set to wake up, but then not be reported as ready immediately after the wake up
+    // (see https://github.com/ros2/rmw_connext/issues/201)
+    // If no other graph events occur, the wait set will not be triggered again until the timeout
+    // has been reached, despite the service being available, so we artificially limit the wait
+    // time to limit the delay.
+    node_ptr->wait_for_graph_change(
+      event, std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100))));
+    // Because of the aforementioned race condition, we check if the service is ready even if the
+    // graph event wasn't triggered.
+    event->check_and_clear();
+    if (this->action_server_is_ready()) {
+      return true;
+    }
+    // server is not ready, loop if there is time left
+    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::Logger
+ClientBase::get_logger()
+{
+  return pimpl_->logger;
+}
+
+size_t
+ClientBase::get_number_of_ready_subscriptions()
+{
+  return pimpl_->num_subscriptions;
+}
+
+size_t
+ClientBase::get_number_of_ready_guard_conditions()
+{
+  return pimpl_->num_guard_conditions;
+}
+
+size_t
+ClientBase::get_number_of_ready_timers()
+{
+  return pimpl_->num_timers;
+}
+
+size_t
+ClientBase::get_number_of_ready_clients()
+{
+  return pimpl_->num_clients;
+}
+
+size_t
+ClientBase::get_number_of_ready_services()
+{
+  return pimpl_->num_services;
+}
+
+bool
+ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+{
+  rcl_ret_t ret = rcl_action_wait_set_add_action_client(
+    wait_set, pimpl_->client_handle.get(), nullptr, nullptr);
+  return RCL_RET_OK == ret;
+}
+
+bool
+ClientBase::is_ready(rcl_wait_set_t * wait_set)
+{
+  rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready(
+    wait_set, pimpl_->client_handle.get(),
+    &pimpl_->is_feedback_ready,
+    &pimpl_->is_status_ready,
+    &pimpl_->is_goal_response_ready,
+    &pimpl_->is_cancel_response_ready,
+    &pimpl_->is_result_response_ready);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "failed to check for any ready entities");
+  }
+  return
+    pimpl_->is_feedback_ready ||
+    pimpl_->is_status_ready ||
+    pimpl_->is_goal_response_ready ||
+    pimpl_->is_cancel_response_ready ||
+    pimpl_->is_result_response_ready;
+}
+
+void
+ClientBase::handle_goal_response(
+  const rmw_request_id_t & response_header,
+  std::shared_ptr<void> response)
+{
+  std::lock_guard<std::mutex> guard(pimpl_->goal_requests_mutex);
+  const int64_t & sequence_number = response_header.sequence_number;
+  if (pimpl_->pending_goal_responses.count(sequence_number) == 0) {
+    RCLCPP_ERROR(pimpl_->logger, "unknown goal response, ignoring...");
+    return;
+  }
+  pimpl_->pending_goal_responses[sequence_number](response);
+  pimpl_->pending_goal_responses.erase(sequence_number);
+}
+
+void
+ClientBase::send_goal_request(std::shared_ptr<void> request, ResponseCallback callback)
+{
+  std::unique_lock<std::mutex> guard(pimpl_->goal_requests_mutex);
+  int64_t sequence_number;
+  rcl_ret_t ret = rcl_action_send_goal_request(
+    pimpl_->client_handle.get(), request.get(), &sequence_number);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request");
+  }
+  assert(pimpl_->pending_goal_responses.count(sequence_number) == 0);
+  pimpl_->pending_goal_responses[sequence_number] = callback;
+}
+
+void
+ClientBase::handle_result_response(
+  const rmw_request_id_t & response_header,
+  std::shared_ptr<void> response)
+{
+  std::lock_guard<std::mutex> guard(pimpl_->result_requests_mutex);
+  const int64_t & sequence_number = response_header.sequence_number;
+  if (pimpl_->pending_result_responses.count(sequence_number) == 0) {
+    RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring...");
+    return;
+  }
+  pimpl_->pending_result_responses[sequence_number](response);
+  pimpl_->pending_result_responses.erase(sequence_number);
+}
+
+void
+ClientBase::send_result_request(std::shared_ptr<void> request, ResponseCallback callback)
+{
+  std::lock_guard<std::mutex> guard(pimpl_->result_requests_mutex);
+  int64_t sequence_number;
+  rcl_ret_t ret = rcl_action_send_result_request(
+    pimpl_->client_handle.get(), request.get(), &sequence_number);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send result request");
+  }
+  assert(pimpl_->pending_result_responses.count(sequence_number) == 0);
+  pimpl_->pending_result_responses[sequence_number] = callback;
+}
+
+void
+ClientBase::handle_cancel_response(
+  const rmw_request_id_t & response_header,
+  std::shared_ptr<void> response)
+{
+  std::lock_guard<std::mutex> guard(pimpl_->cancel_requests_mutex);
+  const int64_t & sequence_number = response_header.sequence_number;
+  if (pimpl_->pending_cancel_responses.count(sequence_number) == 0) {
+    RCLCPP_ERROR(pimpl_->logger, "unknown cancel response, ignoring...");
+    return;
+  }
+  pimpl_->pending_cancel_responses[sequence_number](response);
+  pimpl_->pending_cancel_responses.erase(sequence_number);
+}
+
+void
+ClientBase::send_cancel_request(std::shared_ptr<void> request, ResponseCallback callback)
+{
+  std::lock_guard<std::mutex> guard(pimpl_->cancel_requests_mutex);
+  int64_t sequence_number;
+  rcl_ret_t ret = rcl_action_send_cancel_request(
+    pimpl_->client_handle.get(), request.get(), &sequence_number);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send cancel request");
+  }
+  assert(pimpl_->pending_cancel_responses.count(sequence_number) == 0);
+  pimpl_->pending_cancel_responses[sequence_number] = callback;
+}
+
+GoalID
+ClientBase::generate_goal_id()
+{
+  GoalID goal_id;
+  // TODO(hidmic): Do something better than this for UUID generation.
+  // std::generate(
+  //   goal_id.uuid.begin(), goal_id.uuid.end(),
+  //   std::ref(pimpl_->random_bytes_generator));
+  std::generate(
+    goal_id.begin(), goal_id.end(),
+    std::ref(pimpl_->random_bytes_generator));
+  return goal_id;
+}
+
+void
+ClientBase::execute()
+{
+  if (pimpl_->is_feedback_ready) {
+    std::shared_ptr<void> feedback_message = this->create_feedback_message();
+    rcl_ret_t ret = rcl_action_take_feedback(
+      pimpl_->client_handle.get(), feedback_message.get());
+    pimpl_->is_feedback_ready = false;
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback");
+    } else {
+      this->handle_feedback_message(feedback_message);
+    }
+  } else if (pimpl_->is_status_ready) {
+    std::shared_ptr<void> status_message = this->create_status_message();
+    rcl_ret_t ret = rcl_action_take_status(
+      pimpl_->client_handle.get(), status_message.get());
+    pimpl_->is_status_ready = false;
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status");
+    } else {
+      this->handle_status_message(status_message);
+    }
+  } else if (pimpl_->is_goal_response_ready) {
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> goal_response = this->create_goal_response();
+    rcl_ret_t ret = rcl_action_take_goal_response(
+      pimpl_->client_handle.get(), &response_header, goal_response.get());
+    pimpl_->is_goal_response_ready = false;
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response");
+    } else {
+      this->handle_goal_response(response_header, goal_response);
+    }
+  } else if (pimpl_->is_result_response_ready) {
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> result_response = this->create_result_response();
+    rcl_ret_t ret = rcl_action_take_result_response(
+      pimpl_->client_handle.get(), &response_header, result_response.get());
+    pimpl_->is_result_response_ready = false;
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response");
+    } else {
+      this->handle_result_response(response_header, result_response);
+    }
+  } else if (pimpl_->is_cancel_response_ready) {
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> cancel_response = this->create_cancel_response();
+    rcl_ret_t ret = rcl_action_take_cancel_response(
+      pimpl_->client_handle.get(), &response_header, cancel_response.get());
+    pimpl_->is_cancel_response_ready = false;
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response");
+    } else {
+      this->handle_cancel_response(response_header, cancel_response);
+    }
+  } else {
+    throw std::runtime_error("Executing action client but nothing is ready");
+  }
+}
+
+}  // namespace rclcpp_action

rclcpp_action/src/server.cpp

@@ -0,0 +1,546 @@
+// 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 <rcl_action/action_server.h>
+#include <rcl_action/wait.h>
+
+#include <action_msgs/msg/goal_status_array.hpp>
+#include <action_msgs/srv/cancel_goal.hpp>
+#include <rclcpp/exceptions.hpp>
+#include <rclcpp/scope_exit.hpp>
+#include <rclcpp_action/server.hpp>
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+using rclcpp_action::ServerBase;
+using rclcpp_action::GoalID;
+
+namespace rclcpp_action
+{
+class ServerBaseImpl
+{
+public:
+  ServerBaseImpl(
+    rclcpp::Clock::SharedPtr clock,
+    rclcpp::Logger logger
+  )
+  : clock_(clock), logger_(logger)
+  {
+  }
+
+  // Lock everything except user callbacks
+  std::recursive_mutex reentrant_mutex_;
+
+  std::shared_ptr<rcl_action_server_t> action_server_;
+
+  rclcpp::Clock::SharedPtr clock_;
+
+  size_t num_subscriptions_ = 0;
+  size_t num_timers_ = 0;
+  size_t num_clients_ = 0;
+  size_t num_services_ = 0;
+  size_t num_guard_conditions_ = 0;
+
+  bool goal_request_ready_ = false;
+  bool cancel_request_ready_ = false;
+  bool result_request_ready_ = false;
+  bool goal_expired_ = false;
+
+  // Results to be kept until the goal expires after reaching a terminal state
+  std::unordered_map<GoalID, std::shared_ptr<void>> goal_results_;
+  // Requests for results are kept until a result becomes available
+  std::unordered_map<GoalID, std::vector<rmw_request_id_t>> result_requests_;
+  // rcl goal handles are kept so api to send result doesn't try to access freed memory
+  std::unordered_map<GoalID, std::shared_ptr<rcl_action_goal_handle_t>> goal_handles_;
+
+  rclcpp::Logger logger_;
+};
+}  // namespace rclcpp_action
+
+ServerBase::ServerBase(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+  const std::string & name,
+  const rosidl_action_type_support_t * type_support,
+  const rcl_action_server_options_t & options
+)
+: pimpl_(new ServerBaseImpl(
+      node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action")))
+{
+  auto deleter = [node_base](rcl_action_server_t * ptr)
+    {
+      if (nullptr != ptr) {
+        rcl_node_t * rcl_node = node_base->get_rcl_node_handle();
+        rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node);
+        (void)ret;
+        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"),
+          "failed to fini rcl_action_server_t in deleter");
+      }
+      delete ptr;
+    };
+
+  pimpl_->action_server_.reset(new rcl_action_server_t, deleter);
+  *(pimpl_->action_server_) = rcl_action_get_zero_initialized_server();
+
+  rcl_node_t * rcl_node = node_base->get_rcl_node_handle();
+  rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle();
+
+  rcl_ret_t ret = rcl_action_server_init(
+    pimpl_->action_server_.get(), rcl_node, rcl_clock, type_support, name.c_str(), &options);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  ret = rcl_action_server_wait_set_get_num_entities(
+    pimpl_->action_server_.get(),
+    &pimpl_->num_subscriptions_,
+    &pimpl_->num_guard_conditions_,
+    &pimpl_->num_timers_,
+    &pimpl_->num_clients_,
+    &pimpl_->num_services_);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+ServerBase::~ServerBase()
+{
+}
+
+size_t
+ServerBase::get_number_of_ready_subscriptions()
+{
+  return pimpl_->num_subscriptions_;
+}
+
+size_t
+ServerBase::get_number_of_ready_timers()
+{
+  return pimpl_->num_timers_;
+}
+
+size_t
+ServerBase::get_number_of_ready_clients()
+{
+  return pimpl_->num_clients_;
+}
+
+size_t
+ServerBase::get_number_of_ready_services()
+{
+  return pimpl_->num_services_;
+}
+
+size_t
+ServerBase::get_number_of_ready_guard_conditions()
+{
+  return pimpl_->num_guard_conditions_;
+}
+
+bool
+ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  rcl_ret_t ret = rcl_action_wait_set_add_action_server(
+    wait_set, pimpl_->action_server_.get(), NULL);
+  return RCL_RET_OK == ret;
+}
+
+bool
+ServerBase::is_ready(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready(
+    wait_set,
+    pimpl_->action_server_.get(),
+    &pimpl_->goal_request_ready_,
+    &pimpl_->cancel_request_ready_,
+    &pimpl_->result_request_ready_,
+    &pimpl_->goal_expired_);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  return pimpl_->goal_request_ready_ ||
+         pimpl_->cancel_request_ready_ ||
+         pimpl_->result_request_ready_ ||
+         pimpl_->goal_expired_;
+}
+
+void
+ServerBase::execute()
+{
+  if (pimpl_->goal_request_ready_) {
+    execute_goal_request_received();
+  } else if (pimpl_->cancel_request_ready_) {
+    execute_cancel_request_received();
+  } else if (pimpl_->result_request_ready_) {
+    execute_result_request_received();
+  } else if (pimpl_->goal_expired_) {
+    execute_check_expired_goals();
+  } else {
+    throw std::runtime_error("Executing action server but nothing is ready");
+  }
+}
+
+void
+ServerBase::execute_goal_request_received()
+{
+  rcl_ret_t ret;
+  rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
+  rmw_request_id_t request_header;
+
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+
+  std::shared_ptr<void> message = create_goal_request();
+  ret = rcl_action_take_goal_request(
+    pimpl_->action_server_.get(),
+    &request_header,
+    message.get());
+
+  pimpl_->goal_request_ready_ = false;
+
+  if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
+    // Ignore take failure because connext fails if it receives a sample without valid data.
+    // This happens when a client shuts down and connext receives a sample saying the client is
+    // no longer alive.
+    return;
+  } else if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  GoalID uuid = get_goal_id_from_goal_request(message.get());
+  convert(uuid, &goal_info);
+
+  // Call user's callback, getting the user's response and a ros message to send back
+  auto response_pair = call_handle_goal_callback(uuid, message);
+
+  ret = rcl_action_send_goal_response(
+    pimpl_->action_server_.get(),
+    &request_header,
+    response_pair.second.get());
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  const auto status = response_pair.first;
+
+  // if goal is accepted, create a goal handle, and store it
+  if (GoalResponse::ACCEPT_AND_EXECUTE == status || GoalResponse::ACCEPT_AND_DEFER == status) {
+    RCLCPP_DEBUG(pimpl_->logger_, "Accepted goal %s", to_string(uuid).c_str());
+    // rcl_action will set time stamp
+    auto deleter = [](rcl_action_goal_handle_t * ptr)
+      {
+        if (nullptr != ptr) {
+          rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr);
+          (void)fail_ret;
+          RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"),
+            "failed to fini rcl_action_goal_handle_t in deleter");
+          delete ptr;
+        }
+      };
+    rcl_action_goal_handle_t * rcl_handle;
+    rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info);
+    if (!rcl_handle) {
+      throw std::runtime_error("Failed to accept new goal\n");
+    }
+
+    std::shared_ptr<rcl_action_goal_handle_t> handle(new rcl_action_goal_handle_t, deleter);
+    // Copy out goal handle since action server storage disappears when it is fini'd
+    *handle = *rcl_handle;
+
+    pimpl_->goal_handles_[uuid] = handle;
+
+    if (GoalResponse::ACCEPT_AND_EXECUTE == status) {
+      // Change status to executing
+      ret = rcl_action_update_goal_state(handle.get(), GOAL_EVENT_EXECUTE);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    }
+    // publish status since a goal's state has changed (was accepted or has begun execution)
+    publish_status();
+
+    // Tell user to start executing action
+    call_goal_accepted_callback(handle, uuid, message);
+  }
+}
+
+void
+ServerBase::execute_cancel_request_received()
+{
+  rcl_ret_t ret;
+  rmw_request_id_t request_header;
+
+  // Initialize cancel request
+  auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
+
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  ret = rcl_action_take_cancel_request(
+    pimpl_->action_server_.get(),
+    &request_header,
+    request.get());
+
+  pimpl_->cancel_request_ready_ = false;
+
+  if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
+    // Ignore take failure because connext fails if it receives a sample without valid data.
+    // This happens when a client shuts down and connext receives a sample saying the client is
+    // no longer alive.
+    return;
+  } else if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  // Convert c++ message to C message
+  rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
+  convert(request->goal_info.goal_id.uuid, &cancel_request.goal_info);
+  cancel_request.goal_info.stamp.sec = request->goal_info.stamp.sec;
+  cancel_request.goal_info.stamp.nanosec = request->goal_info.stamp.nanosec;
+
+  // Get a list of goal info that should be attempted to be cancelled
+  rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response();
+
+  ret = rcl_action_process_cancel_request(
+    pimpl_->action_server_.get(),
+    &cancel_request,
+    &cancel_response);
+
+  RCLCPP_SCOPE_EXIT({
+    ret = rcl_action_cancel_response_fini(&cancel_response);
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(pimpl_->logger_, "Failed to fini cancel response");
+    }
+  });
+
+  auto response = std::make_shared<action_msgs::srv::CancelGoal::Response>();
+
+  auto & goals = cancel_response.msg.goals_canceling;
+  // For each canceled goal, call cancel callback
+  for (size_t i = 0; i < goals.size; ++i) {
+    const rcl_action_goal_info_t & goal_info = goals.data[i];
+    GoalID uuid;
+    convert(goal_info, &uuid);
+    auto response_code = call_handle_cancel_callback(uuid);
+    if (CancelResponse::ACCEPT == response_code) {
+      action_msgs::msg::GoalInfo cpp_info;
+      cpp_info.goal_id.uuid = uuid;
+      cpp_info.stamp.sec = goal_info.stamp.sec;
+      cpp_info.stamp.nanosec = goal_info.stamp.nanosec;
+      response->goals_canceling.push_back(cpp_info);
+    }
+  }
+
+  if (!response->goals_canceling.empty()) {
+    // at least one goal state changed, publish a new status message
+    publish_status();
+  }
+
+  ret = rcl_action_send_cancel_response(
+    pimpl_->action_server_.get(), &request_header, response.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerBase::execute_result_request_received()
+{
+  rcl_ret_t ret;
+  // Get the result request message
+  rmw_request_id_t request_header;
+  std::shared_ptr<void> result_request = create_result_request();
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  ret = rcl_action_take_result_request(
+    pimpl_->action_server_.get(), &request_header, result_request.get());
+
+  pimpl_->result_request_ready_ = false;
+
+  if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) {
+    // Ignore take failure because connext fails if it receives a sample without valid data.
+    // This happens when a client shuts down and connext receives a sample saying the client is
+    // no longer alive.
+    return;
+  } else if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  std::shared_ptr<void> result_response;
+
+  // check if the goal exists
+  GoalID uuid = get_goal_id_from_result_request(result_request.get());
+  rcl_action_goal_info_t goal_info;
+  convert(uuid, &goal_info);
+  bool goal_exists;
+  goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+  if (!goal_exists) {
+    // Goal does not exists
+    result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
+  } else {
+    // Goal exists, check if a result is already available
+    auto iter = pimpl_->goal_results_.find(uuid);
+    if (iter != pimpl_->goal_results_.end()) {
+      result_response = iter->second;
+    }
+  }
+
+  if (result_response) {
+    // Send the result now
+    ret = rcl_action_send_result_response(
+      pimpl_->action_server_.get(), &request_header, result_response.get());
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+  } else {
+    // Store the request so it can be responded to later
+    pimpl_->result_requests_[uuid].push_back(request_header);
+  }
+}
+
+void
+ServerBase::execute_check_expired_goals()
+{
+  // Allocate expecting only one goal to expire at a time
+  rcl_action_goal_info_t expired_goals[1];
+  size_t num_expired = 1;
+
+  // Loop in case more than 1 goal expired
+  while (num_expired > 0u) {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+    rcl_ret_t ret;
+    ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    } else if (num_expired) {
+      // A goal expired!
+      GoalID uuid;
+      convert(expired_goals[0], &uuid);
+      RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str());
+      pimpl_->goal_results_.erase(uuid);
+      pimpl_->result_requests_.erase(uuid);
+      pimpl_->goal_handles_.erase(uuid);
+    }
+  }
+}
+
+void
+ServerBase::publish_status()
+{
+  rcl_ret_t ret;
+
+  // Get all goal handles known to C action server
+  rcl_action_goal_handle_t ** goal_handles = NULL;
+  size_t num_goals = 0;
+  ret = rcl_action_server_get_goal_handles(
+    pimpl_->action_server_.get(), &goal_handles, &num_goals);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  auto status_msg = std::make_shared<action_msgs::msg::GoalStatusArray>();
+  status_msg->status_list.reserve(num_goals);
+  // Populate a c++ status message with the goals and their statuses
+  rcl_action_goal_status_array_t c_status_array =
+    rcl_action_get_zero_initialized_goal_status_array();
+  ret = rcl_action_get_goal_status_array(pimpl_->action_server_.get(), &c_status_array);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  RCLCPP_SCOPE_EXIT({
+    ret = rcl_action_goal_status_array_fini(&c_status_array);
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(pimpl_->logger_, "Failed to fini status array message");
+    }
+  });
+
+  for (size_t i = 0; i < c_status_array.msg.status_list.size; ++i) {
+    auto & c_status_msg = c_status_array.msg.status_list.data[i];
+
+    action_msgs::msg::GoalStatus msg;
+    msg.status = c_status_msg.status;
+    // Convert C goal info to C++ goal info
+    convert(c_status_msg.goal_info, &msg.goal_info.goal_id.uuid);
+    msg.goal_info.stamp.sec = c_status_msg.goal_info.stamp.sec;
+    msg.goal_info.stamp.nanosec = c_status_msg.goal_info.stamp.nanosec;
+
+    status_msg->status_list.push_back(msg);
+  }
+
+  // Publish the message through the status publisher
+  ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get());
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerBase::publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg)
+{
+  // Check that the goal exists
+  rcl_action_goal_info_t goal_info;
+  convert(uuid, &goal_info);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  bool goal_exists;
+  goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+
+  if (!goal_exists) {
+    throw std::runtime_error("Asked to publish result for goal that does not exist");
+  }
+
+  pimpl_->goal_results_[uuid] = result_msg;
+
+  // if there are clients who already asked for the result, send it to them
+  auto iter = pimpl_->result_requests_.find(uuid);
+  if (iter != pimpl_->result_requests_.end()) {
+    for (auto & request_header : iter->second) {
+      rcl_ret_t ret = rcl_action_send_result_response(
+        pimpl_->action_server_.get(), &request_header, result_msg.get());
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    }
+  }
+}
+
+void
+ServerBase::notify_goal_terminal_state()
+{
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  rcl_ret_t ret = rcl_action_notify_goal_done(pimpl_->action_server_.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerBase::publish_feedback(std::shared_ptr<void> feedback_msg)
+{
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  rcl_ret_t ret = rcl_action_publish_feedback(pimpl_->action_server_.get(), feedback_msg.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback");
+  }
+}

rclcpp_action/src/server_goal_handle.cpp

@@ -0,0 +1,140 @@
+// 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 <rcl_action/action_server.h>
+#include <rcl_action/goal_handle.h>
+
+#include <rclcpp_action/server_goal_handle.hpp>
+#include <rclcpp/exceptions.hpp>
+
+#include <memory>
+
+namespace rclcpp_action
+{
+ServerGoalHandleBase::~ServerGoalHandleBase()
+{
+}
+
+bool
+ServerGoalHandleBase::is_canceling() const
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
+  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
+  }
+  return GOAL_STATE_CANCELING == state;
+}
+
+bool
+ServerGoalHandleBase::is_active() const
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  return rcl_action_goal_handle_is_active(rcl_handle_.get());
+}
+
+bool
+ServerGoalHandleBase::is_executing() const
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
+  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
+  }
+  return GOAL_STATE_EXECUTING == state;
+}
+
+void
+ServerGoalHandleBase::_set_aborted()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_ABORTED);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_succeeded()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_SUCCEEDED);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_canceling()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_canceled()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_executing()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+bool
+ServerGoalHandleBase::try_canceling() noexcept
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret;
+  // Check if the goal is cancelable
+  const bool is_cancelable = rcl_action_goal_handle_is_cancelable(rcl_handle_.get());
+  if (is_cancelable) {
+    // Transition to CANCELING
+    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL);
+    if (RCL_RET_OK != ret) {
+      return false;
+    }
+  }
+
+  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
+  // Get the current state
+  ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    return false;
+  }
+
+  // If it's canceling, cancel it
+  if (GOAL_STATE_CANCELING == state) {
+    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED);
+    return RCL_RET_OK == ret;
+  }
+
+  return false;
+}
+}  // namespace rclcpp_action

rclcpp_action/src/types.cpp

@@ -0,0 +1,48 @@
+// 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_action/types.hpp"
+
+#include <string>
+#include <sstream>
+
+namespace rclcpp_action
+{
+std::string
+to_string(const GoalID & goal_id)
+{
+  std::stringstream stream;
+  stream << std::hex;
+  for (const auto & element : goal_id) {
+    stream << static_cast<int>(element);
+  }
+  return stream.str();
+}
+
+void
+convert(const GoalID & goal_id, rcl_action_goal_info_t * info)
+{
+  for (size_t i = 0; i < 16; ++i) {
+    info->goal_id.uuid[i] = goal_id[i];
+  }
+}
+
+void
+convert(const rcl_action_goal_info_t & info, GoalID * goal_id)
+{
+  for (size_t i = 0; i < 16; ++i) {
+    (*goal_id)[i] = info.goal_id.uuid[i];
+  }
+}
+}  // namespace rclcpp_action

rclcpp_action/test/test_client.cpp

@@ -0,0 +1,420 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <rcl/allocator.h>
+#include <rcl/time.h>
+#include <rcl/types.h>
+
+#include <rcl_action/names.h>
+#include <rcl_action/default_qos.h>
+
+#include <rclcpp/clock.hpp>
+#include <rclcpp/exceptions.hpp>
+#include <rclcpp/executors.hpp>
+#include <rclcpp/executors/multi_threaded_executor.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/service.hpp>
+#include <rclcpp/time.hpp>
+
+#include <test_msgs/action/fibonacci.hpp>
+
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <thread>
+#include <chrono>
+
+#include "rclcpp_action/exceptions.hpp"
+#include "rclcpp_action/create_client.hpp"
+#include "rclcpp_action/client.hpp"
+#include "rclcpp_action/types.hpp"
+
+using namespace std::chrono_literals;
+
+class TestClient : public ::testing::Test
+{
+protected:
+  using ActionType = test_msgs::action::Fibonacci;
+  using ActionGoal = ActionType::Goal;
+  using ActionGoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
+  using ActionGoalRequestService = ActionType::GoalRequestService;
+  using ActionGoalRequest = ActionGoalRequestService::Request;
+  using ActionGoalResponse = ActionGoalRequestService::Response;
+  using ActionGoalResultService = ActionType::GoalResultService;
+  using ActionGoalResultRequest = ActionGoalResultService::Request;
+  using ActionGoalResultResponse = ActionGoalResultService::Response;
+  using ActionCancelGoalService = ActionType::CancelGoalService;
+  using ActionCancelGoalRequest = ActionType::CancelGoalService::Request;
+  using ActionCancelGoalResponse = ActionType::CancelGoalService::Response;
+  using ActionStatusMessage = ActionType::GoalStatusMessage;
+  using ActionFeedbackMessage = ActionType::Feedback;
+  using ActionFeedback = ActionType::Feedback;
+
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  void SetUp()
+  {
+    rcl_allocator_t allocator = rcl_get_default_allocator();
+
+    server_node = std::make_shared<rclcpp::Node>(server_node_name, namespace_name);
+
+    char * goal_service_name = nullptr;
+    rcl_ret_t ret = rcl_action_get_goal_service_name(
+      action_name, allocator, &goal_service_name);
+    ASSERT_EQ(RCL_RET_OK, ret);
+    goal_service = server_node->create_service<ActionGoalRequestService>(
+      goal_service_name,
+      [this](
+        const ActionGoalRequest::SharedPtr request,
+        ActionGoalResponse::SharedPtr response)
+      {
+        response->stamp = clock.now();
+        response->accepted = (request->order >= 0);
+        if (response->accepted) {
+          goals[request->action_goal_id.uuid] = {request, response};
+        }
+      });
+    ASSERT_TRUE(goal_service != nullptr);
+    allocator.deallocate(goal_service_name, allocator.state);
+
+    char * result_service_name = nullptr;
+    ret = rcl_action_get_result_service_name(
+      action_name, allocator, &result_service_name);
+    ASSERT_EQ(RCL_RET_OK, ret);
+    result_service = server_node->create_service<ActionGoalResultService>(
+      result_service_name,
+      [this](
+        const ActionGoalResultRequest::SharedPtr request,
+        ActionGoalResultResponse::SharedPtr response)
+      {
+        if (goals.count(request->action_goal_id.uuid) == 1) {
+          auto goal_request = goals[request->action_goal_id.uuid].first;
+          auto goal_response = goals[request->action_goal_id.uuid].second;
+          ActionStatusMessage status_message;
+          rclcpp_action::GoalStatus goal_status;
+          goal_status.goal_info.goal_id.uuid = goal_request->action_goal_id.uuid;
+          goal_status.goal_info.stamp = goal_response->stamp;
+          goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING;
+          status_message.status_list.push_back(goal_status);
+          status_publisher->publish(status_message);
+          client_executor.spin_once();
+          ActionFeedbackMessage feedback_message;
+          feedback_message.action_goal_id.uuid = goal_request->action_goal_id.uuid;
+          feedback_message.sequence.push_back(0);
+          feedback_publisher->publish(feedback_message);
+          client_executor.spin_once();
+          if (goal_request->order > 0) {
+            feedback_message.sequence.push_back(1);
+            feedback_publisher->publish(feedback_message);
+            client_executor.spin_once();
+            for (int i = 1; i < goal_request->order; ++i) {
+              feedback_message.sequence.push_back(
+                feedback_message.sequence[i] + feedback_message.sequence[i - 1]);
+              feedback_publisher->publish(feedback_message);
+              client_executor.spin_once();
+            }
+          }
+          goal_status.status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
+          status_message.status_list[0] = goal_status;
+          status_publisher->publish(status_message);
+          client_executor.spin_once();
+          response->sequence = feedback_message.sequence;
+          response->action_status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
+          goals.erase(request->action_goal_id.uuid);
+        } else {
+          response->action_status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
+        }
+      });
+    ASSERT_TRUE(result_service != nullptr);
+    allocator.deallocate(result_service_name, allocator.state);
+
+    char * cancel_service_name = nullptr;
+    ret = rcl_action_get_cancel_service_name(
+      action_name, allocator, &cancel_service_name);
+    ASSERT_EQ(RCL_RET_OK, ret);
+    cancel_service = server_node->create_service<ActionCancelGoalService>(
+      cancel_service_name,
+      [this](
+        const ActionCancelGoalRequest::SharedPtr request,
+        ActionCancelGoalResponse::SharedPtr response)
+      {
+        rclcpp_action::GoalID zero_uuid;
+        std::fill(zero_uuid.begin(), zero_uuid.end(), 0u);
+        const rclcpp::Time cancel_stamp = request->goal_info.stamp;
+        bool cancel_all = (
+          request->goal_info.goal_id.uuid == zero_uuid &&
+          cancel_stamp == zero_stamp);
+        ActionStatusMessage status_message;
+        auto it = goals.begin();
+        while (it != goals.end()) {
+          auto goal_request = it->second.first;
+          auto goal_response = it->second.second;
+          const rclcpp::Time goal_stamp = goal_response->stamp;
+          bool cancel_this = (
+            request->goal_info.goal_id.uuid == goal_request->action_goal_id.uuid ||
+            cancel_stamp > goal_stamp);
+          if (cancel_all || cancel_this) {
+            rclcpp_action::GoalStatus goal_status;
+            goal_status.goal_info.goal_id.uuid = goal_request->action_goal_id.uuid;
+            goal_status.goal_info.stamp = goal_response->stamp;
+            goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED;
+            status_message.status_list.push_back(goal_status);
+            response->goals_canceling.push_back(goal_status.goal_info);
+            it = goals.erase(it);
+          } else {
+            ++it;
+          }
+        }
+        status_publisher->publish(status_message);
+        client_executor.spin_once();
+      });
+    ASSERT_TRUE(cancel_service != nullptr);
+    allocator.deallocate(cancel_service_name, allocator.state);
+
+    char * feedback_topic_name = nullptr;
+    ret = rcl_action_get_feedback_topic_name(
+      action_name, allocator, &feedback_topic_name);
+    ASSERT_EQ(RCL_RET_OK, ret);
+    feedback_publisher = server_node->create_publisher<ActionFeedbackMessage>(feedback_topic_name);
+    ASSERT_TRUE(feedback_publisher != nullptr);
+    allocator.deallocate(feedback_topic_name, allocator.state);
+
+    char * status_topic_name = nullptr;
+    ret = rcl_action_get_status_topic_name(
+      action_name, allocator, &status_topic_name);
+    ASSERT_EQ(RCL_RET_OK, ret);
+    status_publisher = server_node->create_publisher<ActionStatusMessage>(
+      status_topic_name, rcl_action_qos_profile_status_default);
+    ASSERT_TRUE(status_publisher != nullptr);
+    allocator.deallocate(status_topic_name, allocator.state);
+    server_executor.add_node(server_node);
+
+    client_node = std::make_shared<rclcpp::Node>(client_node_name, namespace_name);
+    client_executor.add_node(client_node);
+
+    ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(clock.get_clock_handle()));
+    ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1)));
+  }
+
+  void Teardown()
+  {
+    status_publisher.reset();
+    feedback_publisher.reset();
+    cancel_service.reset();
+    result_service.reset();
+    goal_service.reset();
+    server_node.reset();
+    client_node.reset();
+  }
+
+  template<typename FutureT>
+  void dual_spin_until_future_complete(std::shared_future<FutureT> & future)
+  {
+    std::future_status status;
+    do {
+      server_executor.spin_some();
+      client_executor.spin_some();
+      status = future.wait_for(std::chrono::seconds(0));
+    } while (std::future_status::ready != status);
+  }
+
+  rclcpp::Clock clock{RCL_ROS_TIME};
+  const rclcpp::Time zero_stamp{0, 0, RCL_ROS_TIME};
+
+  rclcpp::Node::SharedPtr server_node;
+  rclcpp::executors::SingleThreadedExecutor server_executor;
+  rclcpp::Node::SharedPtr client_node;
+  rclcpp::executors::SingleThreadedExecutor client_executor;
+  const char * const server_node_name{"fibonacci_action_test_server"};
+  const char * const client_node_name{"fibonacci_action_test_client"};
+  const char * const namespace_name{"/rclcpp_action/test/client"};
+  const char * const action_name{"fibonacci_test"};
+
+  std::map<
+    rclcpp_action::GoalID,
+    std::pair<
+      typename ActionGoalRequest::SharedPtr,
+      typename ActionGoalResponse::SharedPtr>> goals;
+  typename rclcpp::Service<ActionGoalRequestService>::SharedPtr goal_service;
+  typename rclcpp::Service<ActionGoalResultService>::SharedPtr result_service;
+  typename rclcpp::Service<ActionCancelGoalService>::SharedPtr cancel_service;
+  typename rclcpp::Publisher<ActionFeedbackMessage>::SharedPtr feedback_publisher;
+  typename rclcpp::Publisher<ActionStatusMessage>::SharedPtr status_publisher;
+};
+
+TEST_F(TestClient, construction_and_destruction)
+{
+  ASSERT_NO_THROW(rclcpp_action::create_client<ActionType>(client_node, action_name).reset());
+}
+
+TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal bad_goal;
+  bad_goal.order = -5;
+  auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle);
+  EXPECT_EQ(nullptr, future_goal_handle.get().get());
+
+  ActionGoal good_goal;
+  good_goal.order = 5;
+  future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_FALSE(goal_handle->is_feedback_aware());
+  EXPECT_FALSE(goal_handle->is_result_aware());
+  EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError);
+}
+
+TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal goal;
+  goal.order = 5;
+  auto future_goal_handle = action_client->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_FALSE(goal_handle->is_feedback_aware());
+  EXPECT_TRUE(goal_handle->is_result_aware());
+  auto future_result = goal_handle->async_result();
+  dual_spin_until_future_complete(future_result);
+  auto result = future_result.get();
+  ASSERT_EQ(6ul, result.response->sequence.size());
+  EXPECT_EQ(0, result.response->sequence[0]);
+  EXPECT_EQ(1, result.response->sequence[1]);
+  EXPECT_EQ(5, result.response->sequence[5]);
+}
+
+TEST_F(TestClient, async_send_goal_with_feedback_and_result)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal goal;
+  goal.order = 4;
+  int feedback_count = 0;
+  auto future_goal_handle = action_client->async_send_goal(
+    goal,
+    [&feedback_count](
+      typename ActionGoalHandle::SharedPtr goal_handle,
+      const std::shared_ptr<const ActionFeedback> feedback) mutable
+    {
+      (void)goal_handle;
+      (void)feedback;
+      feedback_count++;
+    });
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_TRUE(goal_handle->is_feedback_aware());
+  EXPECT_TRUE(goal_handle->is_result_aware());
+  auto future_result = goal_handle->async_result();
+  dual_spin_until_future_complete(future_result);
+  auto result = future_result.get();
+
+  ASSERT_EQ(5u, result.response->sequence.size());
+  EXPECT_EQ(3, result.response->sequence.back());
+  EXPECT_EQ(5, feedback_count);
+}
+
+TEST_F(TestClient, async_cancel_one_goal)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal goal;
+  goal.order = 5;
+  auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+
+  auto future_cancel = action_client->async_cancel_goal(goal_handle);
+  dual_spin_until_future_complete(future_cancel);
+  bool goal_canceled = future_cancel.get();
+  EXPECT_TRUE(goal_canceled);
+}
+
+TEST_F(TestClient, async_cancel_all_goals)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal goal;
+  goal.order = 6;
+  auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle0);
+  auto goal_handle0 = future_goal_handle0.get();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
+
+  goal.order = 8;
+  auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle1);
+  auto goal_handle1 = future_goal_handle1.get();
+
+  if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) {
+    goal_handle0.swap(goal_handle1);
+  }
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
+
+  auto future_cancel_all = action_client->async_cancel_all_goals();
+  dual_spin_until_future_complete(future_cancel_all);
+  auto cancel_response = future_cancel_all.get();
+
+  ASSERT_EQ(2ul, cancel_response->goals_canceling.size());
+  EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
+  EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
+}
+
+TEST_F(TestClient, async_cancel_some_goals)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal goal;
+  goal.order = 6;
+  auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle0);
+  auto goal_handle0 = future_goal_handle0.get();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
+
+  goal.order = 8;
+  auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle1);
+  auto goal_handle1 = future_goal_handle1.get();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
+
+  auto future_cancel_some =
+    action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp());
+  dual_spin_until_future_complete(future_cancel_some);
+  auto cancel_response = future_cancel_some.get();
+
+  ASSERT_EQ(1ul, cancel_response->goals_canceling.size());
+  EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
+}

rclcpp_action/test/test_server.cpp

@@ -0,0 +1,643 @@
+// 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/exceptions.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <test_msgs/action/fibonacci.hpp>
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <vector>
+
+#include "rclcpp_action/create_server.hpp"
+#include "rclcpp_action/server.hpp"
+
+using Fibonacci = test_msgs::action::Fibonacci;
+using GoalID = rclcpp_action::GoalID;
+
+class TestServer : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  std::shared_ptr<Fibonacci::GoalRequestService::Request>
+  send_goal_request(rclcpp::Node::SharedPtr node, GoalID uuid)
+  {
+    auto client = node->create_client<Fibonacci::GoalRequestService>(
+      "fibonacci/_action/send_goal");
+    if (!client->wait_for_service(std::chrono::seconds(20))) {
+      throw std::runtime_error("send goal service didn't become available");
+    }
+    auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();
+    request->action_goal_id.uuid = uuid;
+    auto future = client->async_send_request(request);
+    if (rclcpp::executor::FutureReturnCode::SUCCESS !=
+      rclcpp::spin_until_future_complete(node, future))
+    {
+      throw std::runtime_error("send goal future didn't complete succesfully");
+    }
+    return request;
+  }
+
+  void
+  send_cancel_request(rclcpp::Node::SharedPtr node, GoalID uuid)
+  {
+    auto cancel_client = node->create_client<Fibonacci::CancelGoalService>(
+      "fibonacci/_action/cancel_goal");
+    if (!cancel_client->wait_for_service(std::chrono::seconds(20))) {
+      throw std::runtime_error("cancel goal service didn't become available");
+    }
+    auto request = std::make_shared<Fibonacci::CancelGoalService::Request>();
+    request->goal_info.goal_id.uuid = uuid;
+    auto future = cancel_client->async_send_request(request);
+    if (rclcpp::executor::FutureReturnCode::SUCCESS !=
+      rclcpp::spin_until_future_complete(node, future))
+    {
+      throw std::runtime_error("cancel goal future didn't complete succesfully");
+    }
+  }
+};
+
+TEST_F(TestServer, construction_and_destruction)
+{
+  auto node = std::make_shared<rclcpp::Node>("construct_node", "/rclcpp_action/construct");
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      [](const GoalID &, std::shared_ptr<const Fibonacci::Goal>) {
+        return rclcpp_action::GoalResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {});
+  (void)as;
+}
+
+TEST_F(TestServer, handle_goal_called)
+{
+  auto node = std::make_shared<rclcpp::Node>("handle_goal_node", "/rclcpp_action/handle_goal");
+  GoalID received_uuid;
+
+  auto handle_goal = [&received_uuid](
+    const GoalID & uuid, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      received_uuid = uuid;
+      return rclcpp_action::GoalResponse::REJECT;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {});
+  (void)as;
+
+  // Create a client that calls the goal request service
+  // Make sure the UUID received is the same as the one sent
+
+  auto client = node->create_client<Fibonacci::GoalRequestService>(
+    "fibonacci/_action/send_goal");
+
+  ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20)));
+
+  auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();
+
+  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+  request->action_goal_id.uuid = uuid;
+
+  auto future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::executor::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future));
+
+  ASSERT_EQ(uuid, received_uuid);
+}
+
+TEST_F(TestServer, handle_accepted_called)
+{
+  auto node = std::make_shared<rclcpp::Node>("handle_exec_node", "/rclcpp_action/handle_accepted");
+  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::REJECT;
+      },
+      handle_accepted);
+  (void)as;
+
+  auto request = send_goal_request(node, uuid);
+
+  ASSERT_TRUE(received_handle);
+  ASSERT_TRUE(received_handle->is_active());
+  EXPECT_EQ(uuid, received_handle->get_goal_id());
+  EXPECT_EQ(*request, *(received_handle->get_goal()));
+}
+
+TEST_F(TestServer, handle_cancel_called)
+{
+  auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel");
+  const GoalID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  send_goal_request(node, uuid);
+
+  ASSERT_TRUE(received_handle);
+  EXPECT_EQ(uuid, received_handle->get_goal_id());
+  EXPECT_FALSE(received_handle->is_canceling());
+
+  send_cancel_request(node, uuid);
+  EXPECT_TRUE(received_handle->is_canceling());
+}
+
+TEST_F(TestServer, publish_status_accepted)
+{
+  auto node = std::make_shared<rclcpp::Node>("status_accept_node", "/rclcpp_action/status_accept");
+  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    {
+      received_msgs.push_back(list);
+    });
+
+  send_goal_request(node, uuid);
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() != 1u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_LT(0u, received_msgs.size());
+  // Not sure whether accepted will come through because not sure when subscriber will match
+  for (auto & msg : received_msgs) {
+    ASSERT_EQ(1u, msg->status_list.size());
+    EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid);
+    auto status = msg->status_list.at(0).status;
+    if (action_msgs::msg::GoalStatus::STATUS_ACCEPTED == status) {
+      EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ACCEPTED, status);
+    } else {
+      EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_EXECUTING, status);
+    }
+  }
+}
+
+TEST_F(TestServer, publish_status_canceling)
+{
+  auto node = std::make_shared<rclcpp::Node>("status_cancel_node", "/rclcpp_action/status_cancel");
+  const GoalID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    {
+      received_msgs.push_back(list);
+    });
+
+  send_goal_request(node, uuid);
+  send_cancel_request(node, uuid);
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_LT(0u, received_msgs.size());
+  auto & msg = received_msgs.back();
+  ASSERT_EQ(1u, msg->status_list.size());
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELING, msg->status_list.at(0).status);
+  EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid);
+}
+
+TEST_F(TestServer, publish_status_canceled)
+{
+  auto node = std::make_shared<rclcpp::Node>("status_canceled", "/rclcpp_action/status_canceled");
+  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    {
+      received_msgs.push_back(list);
+    });
+
+  send_goal_request(node, uuid);
+  send_cancel_request(node, uuid);
+
+  received_handle->set_canceled(std::make_shared<Fibonacci::Result>());
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() < 3u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_LT(0u, received_msgs.size());
+  auto & msg = received_msgs.back();
+  ASSERT_EQ(1u, msg->status_list.size());
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELED, msg->status_list.at(0).status);
+  EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid);
+}
+
+TEST_F(TestServer, publish_status_succeeded)
+{
+  auto node = std::make_shared<rclcpp::Node>("status_succeeded", "/rclcpp_action/status_succeeded");
+  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    {
+      received_msgs.push_back(list);
+    });
+
+  send_goal_request(node, uuid);
+  received_handle->set_succeeded(std::make_shared<Fibonacci::Result>());
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_LT(0u, received_msgs.size());
+  auto & msg = received_msgs.back();
+  ASSERT_EQ(1u, msg->status_list.size());
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, msg->status_list.at(0).status);
+  EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid);
+}
+
+TEST_F(TestServer, publish_status_aborted)
+{
+  auto node = std::make_shared<rclcpp::Node>("status_aborted", "/rclcpp_action/status_aborted");
+  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    {
+      received_msgs.push_back(list);
+    });
+
+  send_goal_request(node, uuid);
+  received_handle->set_aborted(std::make_shared<Fibonacci::Result>());
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_LT(0u, received_msgs.size());
+  auto & msg = received_msgs.back();
+  ASSERT_EQ(1u, msg->status_list.size());
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ABORTED, msg->status_list.at(0).status);
+  EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid);
+}
+
+TEST_F(TestServer, publish_feedback)
+{
+  auto node = std::make_shared<rclcpp::Node>("pub_feedback", "/rclcpp_action/pub_feedback");
+  const GoalID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to feedback messages
+  using FeedbackT = Fibonacci::Feedback;
+  std::vector<FeedbackT::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<FeedbackT>(
+    "fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg)
+    {
+      received_msgs.push_back(msg);
+    });
+
+  send_goal_request(node, uuid);
+
+  auto sent_message = std::make_shared<FeedbackT>();
+  sent_message->sequence = {1, 1, 2, 3, 5};
+  received_handle->publish_feedback(sent_message);
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() < 1u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_EQ(1u, received_msgs.size());
+  auto & msg = received_msgs.back();
+  ASSERT_EQ(sent_message->sequence, msg->sequence);
+  ASSERT_EQ(uuid, msg->action_goal_id.uuid);
+}
+
+TEST_F(TestServer, get_result)
+{
+  auto node = std::make_shared<rclcpp::Node>("get_result", "/rclcpp_action/get_result");
+  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  send_goal_request(node, uuid);
+
+  // Send result request
+  auto result_client = node->create_client<Fibonacci::GoalResultService>(
+    "fibonacci/_action/get_result");
+  if (!result_client->wait_for_service(std::chrono::seconds(20))) {
+    throw std::runtime_error("get result service didn't become available");
+  }
+  auto request = std::make_shared<Fibonacci::GoalResultService::Request>();
+  request->action_goal_id.uuid = uuid;
+  auto future = result_client->async_send_request(request);
+
+  // Send a result
+  auto result = std::make_shared<Fibonacci::Result>();
+  result->sequence = {5, 8, 13, 21};
+  received_handle->set_succeeded(result);
+
+  // Wait for the result request to be received
+  ASSERT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future));
+
+  auto response = future.get();
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->action_status);
+  EXPECT_EQ(result->sequence, response->sequence);
+}
+
+TEST_F(TestServer, deferred_execution)
+{
+  auto node = std::make_shared<rclcpp::Node>("defer_exec", "/rclcpp_action/defer_exec");
+  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<test_msgs::action::Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  send_goal_request(node, uuid);
+
+  EXPECT_TRUE(received_handle->is_active());
+  EXPECT_FALSE(received_handle->is_executing());
+  received_handle->set_executing();
+  EXPECT_TRUE(received_handle->is_executing());
+}

rclcpp_lifecycle/CHANGELOG.rst

@@ -2,6 +2,16 @@
 Changelog for package rclcpp_lifecycle
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.6.2 (2018-12-13)
+------------------
+
+0.6.1 (2018-12-07)
+------------------
+* Added node path and time stamp to parameter event message (`#584 <https://github.com/ros2/rclcpp/issues/584>`_)
+* Refactored init to allow for non-global init (`#587 <https://github.com/ros2/rclcpp/issues/587>`_)
+* Add class Waitable (`#589 <https://github.com/ros2/rclcpp/issues/589>`_)
+* Contributors: Dirk Thomas, Jacob Perron, William Woodall, bpwilcox
+
 0.6.0
 -----------
 * Updated to use new error handling API from rcutils (`#577 <https://github.com/ros2/rclcpp/issues/577>`_)

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -44,6 +44,7 @@
 #include "rclcpp/node_interfaces/node_services_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"
@@ -96,6 +97,7 @@ public:
    * 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.
+   * \param[in] allow_undeclared_parameters True to allow any parameter name to be set on the node.
    */
   RCLCPP_LIFECYCLE_PUBLIC
   LifecycleNode(
@@ -106,7 +108,8 @@ public:
     const std::vector<rclcpp::Parameter> & initial_parameters,
     bool use_global_arguments = true,
     bool use_intra_process_comms = false,
-    bool start_parameter_services = true);
+    bool start_parameter_services = true,
+    bool allow_undeclared_parameters = false);
 
   RCLCPP_LIFECYCLE_PUBLIC
   virtual ~LifecycleNode();
@@ -390,6 +393,11 @@ public:
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr
   get_node_parameters_interface();
 
+  /// Return the Node's internal NodeWaitablesInterface implementation.
+  RCLCPP_LIFECYCLE_PUBLIC
+  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr
+  get_node_waitables_interface();
+
   //
   // LIFECYCLE COMPONENTS
   //
@@ -513,8 +521,9 @@ 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::NodeWaitablesInterface::SharedPtr node_waitables_;
 
   bool use_intra_process_comms_;
 

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp

@@ -141,7 +141,7 @@ LifecycleNode::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;
 }

rclcpp_lifecycle/package.xml

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