another wip

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

rclcpp/CHANGELOG.rst

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

rclcpp/CMakeLists.txt

@@ -53,6 +53,7 @@ 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
@@ -64,6 +65,7 @@ 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
@@ -399,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/context.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__CONTEXT_HPP_
 #define RCLCPP__CONTEXT_HPP_
 
+#include <condition_variable>
 #include <functional>
 #include <memory>
 #include <mutex>
@@ -26,6 +27,8 @@
 #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"
@@ -147,21 +150,21 @@ public:
    * - 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
-   * - if notify_all is true, rclcpp::notify_all is called to unblock some ROS functions
+   * - 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
-   * \param[in] notify_all if true, then rclcpp::notify_all will be called
    * \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, bool notify_all = true);
+  shutdown(const std::string & reason);
 
   using OnShutdownCallback = std::function<void ()>;
 
@@ -170,8 +173,9 @@ public:
    * These callbacks will be called in the order they are added as the second
    * to last step in shutdown().
    *
-   * These callbacks may be run in the signal handler as the signal handler
-   * may call shutdown() on this context.
+   * 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
@@ -211,6 +215,86 @@ 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>
@@ -261,6 +345,16 @@ private:
 
   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.

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

@@ -134,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
@@ -155,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_;

rclcpp/include/rclcpp/node.hpp

@@ -47,6 +47,7 @@
 #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"
@@ -94,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(
@@ -104,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();
@@ -264,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);
@@ -278,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;
@@ -304,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
@@ -458,6 +523,11 @@ 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)
 
@@ -473,6 +543,7 @@ private:
   rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_;
   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

@@ -210,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(
@@ -224,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
@@ -237,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

@@ -61,10 +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
 {
@@ -54,60 +67,65 @@ public:
     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)
@@ -116,7 +134,11 @@ 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_;
 

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_interface.hpp

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

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

@@ -51,7 +51,9 @@ public:
     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::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();
@@ -76,6 +78,8 @@ private:
   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_;

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

@@ -24,9 +24,6 @@
 #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"
 
@@ -184,9 +181,11 @@ shutdown(
  * If nullptr is given for the context, then the global context is used, i.e.
  * the context initialized by rclcpp::init().
  *
- * Note that these callbacks should be non-blocking and not throw exceptions,
- * as they may be called from signal handlers and from the destructor of the
- * context.
+ * 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
@@ -196,53 +195,18 @@ RCLCPP_PUBLIC
 void
 on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr context = nullptr);
 
-/// 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[in] wait_set Pointer to the rcl_wait_set_t that will be using the
- *   resulting guard condition.
- * \param[in] context The context associated with the guard condition.
- * \return Pointer to the guard condition.
- */
-RCLCPP_PUBLIC
-rcl_guard_condition_t *
-get_sigint_guard_condition(rcl_wait_set_t * wait_set, rclcpp::Context::SharedPtr context);
-
-/// Release the previously allocated guard condition that manages the signal handler.
-/**
- * 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.
- *
- * If nullptr is given for the context, then the global context is used, i.e.
- * the context initialized by rclcpp::init().
- *
- * \param[in] wait_set Pointer to the rcl_wait_set_t that was using the
- *   resulting guard condition.
- */
-RCLCPP_PUBLIC
-void
-release_sigint_guard_condition(rcl_wait_set_t * wait_set);
-
 /// 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 rclcpp::shutdown() or the signal handler.
+ * 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.
  * \param[in] context which may interrupt this sleep
- * \return True if the condition variable did not timeout.
+ * \return true if the condition variable did not timeout.
  */
 RCLCPP_PUBLIC
 bool
@@ -250,11 +214,6 @@ sleep_for(
   const std::chrono::nanoseconds & nanoseconds,
   rclcpp::Context::SharedPtr context = nullptr);
 
-/// Notify all blocking calls so they can check for changes in rclcpp::ok().
-RCLCPP_PUBLIC
-void
-notify_all();
-
 /// Safely check if addition will overflow.
 /**
  * The type of the operands, T, should have defined

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.1</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/context.cpp

@@ -22,6 +22,7 @@
 #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;
@@ -127,7 +128,7 @@ Context::shutdown_reason()
 }
 
 bool
-Context::shutdown(const std::string & reason, bool notify_all)
+Context::shutdown(const std::string & reason)
 {
   // prevent races
   std::lock_guard<std::recursive_mutex> init_lock(init_mutex_);
@@ -147,10 +148,9 @@ Context::shutdown(const std::string & reason, bool notify_all)
   for (const auto & callback : on_shutdown_callbacks_) {
     callback();
   }
-  // notify all blocking calls, if asked
-  if (notify_all) {
-    rclcpp::notify_all();
-  }
+  // 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(); ) {
@@ -190,6 +190,99 @@ 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()
 {

rclcpp/src/rclcpp/executor.cpp

@@ -51,8 +51,7 @@ Executor::Executor(const ExecutorArgs & args)
   // 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_, args.context));
+  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_);
@@ -61,9 +60,8 @@ Executor::Executor(const ExecutorArgs & args)
   // Store the context for later use.
   context_ = args.context;
 
-  if (rcl_wait_set_init(
-      &wait_set_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK)
-  {
+  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);
@@ -105,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_, context_));
-  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

rclcpp/src/rclcpp/graph_listener.cpp

@@ -36,7 +36,8 @@ namespace graph_listener
 {
 
 GraphListener::GraphListener(std::shared_ptr<rclcpp::Context> parent_context)
-: is_started_(false),
+: parent_context_(parent_context),
+  is_started_(false),
   is_shutdown_(false),
   interrupt_guard_condition_context_(nullptr),
   shutdown_guard_condition_(nullptr)
@@ -54,12 +55,12 @@ GraphListener::GraphListener(std::shared_ptr<rclcpp::Context> parent_context)
     throw_from_rcl_error(ret, "failed to create interrupt guard condition");
   }
 
-  shutdown_guard_condition_ = rclcpp::get_sigint_guard_condition(&wait_set_, parent_context);
+  shutdown_guard_condition_ = parent_context->get_interrupt_guard_condition(&wait_set_);
 }
 
 GraphListener::~GraphListener()
 {
-  this->shutdown();
+  this->shutdown(std::nothrow);
 }
 
 void
@@ -78,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");
@@ -90,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.
@@ -337,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)) {
@@ -351,7 +354,11 @@ GraphListener::shutdown()
       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_) {
@@ -363,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/node.cpp

@@ -29,6 +29,7 @@
 #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"
@@ -59,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())),
@@ -81,7 +83,17 @@ Node::Node(
       node_clock_,
       initial_parameters,
       use_intra_process_comms,
-      start_parameter_services
+      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)
@@ -122,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)
@@ -263,6 +284,12 @@ 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()
 {

rclcpp/src/rclcpp/node_interfaces/node_clock.cpp

@@ -31,15 +31,7 @@ NodeClock::NodeClock(
   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_,
-    node_logging_);
-  time_source_.attachClock(ros_clock_);
-}
+{}
 
 NodeClock::~NodeClock()
 {}

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -39,8 +39,9 @@ NodeParameters::NodeParameters(
   const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
   const std::vector<rclcpp::Parameter> & initial_parameters,
   bool use_intra_process,
-  bool start_parameter_services)
-: node_clock_(node_clock)
+  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>;
@@ -114,8 +115,6 @@ NodeParameters::NodeParameters(
     combined_name_ = node_namespace + '/' + node_name;
   }
 
-  std::map<std::string, rclcpp::Parameter> parameters;
-
   // TODO(sloretz) use rcl to parse yaml when circular dependency is solved
   // See https://github.com/ros2/rcl/issues/252
   for (const std::string & yaml_path : yaml_paths) {
@@ -140,25 +139,19 @@ NodeParameters::NodeParameters(
 
     // 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");
     }
@@ -168,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)
@@ -185,7 +231,7 @@ 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_;
@@ -202,33 +248,79 @@ 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_);
@@ -246,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;
@@ -275,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
 {
@@ -293,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;
@@ -312,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/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/time_source.cpp

@@ -53,7 +53,9 @@ void TimeSource::attachNode(rclcpp::Node::SharedPtr node)
     node->get_node_topics_interface(),
     node->get_node_graph_interface(),
     node->get_node_services_interface(),
-    node->get_node_logging_interface());
+    node->get_node_logging_interface(),
+    node->get_node_clock_interface(),
+    node->get_node_parameters_interface());
 }
 
 void TimeSource::attachNode(
@@ -61,17 +63,38 @@ void TimeSource::attachNode(
   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::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_,
@@ -92,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();
 }
 
@@ -103,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)
@@ -123,7 +150,9 @@ void TimeSource::detachClock(std::shared_ptr<rclcpp::Clock> 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();
   }
 }
@@ -257,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/utilities.cpp

@@ -14,145 +14,16 @@
 
 #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/logging.hpp"
 #include "rclcpp/exceptions.hpp"
-#include "rclcpp/scope_exit.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
-
-/// Mutex to protect installation and uninstallation of signal handlers.
-static std::mutex g_signal_handlers_mutex;
-/// Atomic bool to control setup of signal handlers.
-static std::atomic<bool> g_signal_handlers_installed(false);
-
-/// Mutex to protect g_sigint_guard_cond_handles
-static std::mutex g_sigint_guard_cond_handles_mutex;
-/// 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;
-
-/// Condition variable for timed sleep (see sleep_for).
-static std::condition_variable g_interrupt_condition_variable;
-/// 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
-{
-#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
-}
-
-void
-#ifdef HAS_SIGACTION
-signal_handler(int signal_value, siginfo_t * siginfo, void * context)
-#else
-signal_handler(int signal_value)
-#endif
-{
-  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "signal_handler(signal_value=%d)", 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
-
-  for (auto context_ptr : rclcpp::get_contexts()) {
-    if (context_ptr->get_init_options().shutdown_on_sigint) {
-      // do not notify all, instead do that once after all are shutdown
-      context_ptr->shutdown("signal handler", false /* notify_all */);
-    }
-  }
-  rclcpp::notify_all();
-}
-
 void
 rclcpp::init(int argc, char const * const argv[], const rclcpp::InitOptions & init_options)
 {
@@ -165,46 +36,19 @@ rclcpp::init(int argc, char const * const argv[], const rclcpp::InitOptions & in
 bool
 rclcpp::install_signal_handlers()
 {
-  std::lock_guard<std::mutex> lock(g_signal_handlers_mutex);
-  bool already_installed = g_signal_handlers_installed.exchange(true);
-  if (already_installed) {
-    return false;
-  }
-#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);
-#else
-  ::old_signal_handler = set_signal_handler(SIGINT, ::signal_handler);
-#endif
-  RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "signal handler installed");
-  return true;
+  return rclcpp::SignalHandler::get_global_signal_handler().install();
 }
 
 bool
 rclcpp::signal_handlers_installed()
 {
-  return g_signal_handlers_installed.load();
+  return rclcpp::SignalHandler::get_global_signal_handler().is_installed();
 }
 
 bool
 rclcpp::uninstall_signal_handlers()
 {
-  std::lock_guard<std::mutex> lock(g_signal_handlers_mutex);
-  bool installed = g_signal_handlers_installed.exchange(false);
-  if (!installed) {
-    return false;
-  }
-#ifdef HAS_SIGACTION
-  set_sigaction(SIGINT, ::old_action);
-#else
-  set_signal_handler(SIGINT, ::old_signal_handler);
-#endif
-  RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "signal handler uninstalled");
-  return true;
+  return rclcpp::SignalHandler::get_global_signal_handler().uninstall();
 }
 
 std::vector<std::string>
@@ -300,14 +144,10 @@ rclcpp::shutdown(rclcpp::Context::SharedPtr context, const std::string & reason)
   if (nullptr == context) {
     context = default_context;
   }
-
   bool ret = context->shutdown(reason);
-
-  // Uninstall the signal handlers if this is the default context's shutdown.
   if (context == default_context) {
-    uninstall_signal_handlers();
+    rclcpp::uninstall_signal_handlers();
   }
-
   return ret;
 }
 
@@ -321,84 +161,14 @@ rclcpp::on_shutdown(std::function<void()> callback, rclcpp::Context::SharedPtr c
   context->on_shutdown(callback);
 }
 
-rcl_guard_condition_t *
-rclcpp::get_sigint_guard_condition(rcl_wait_set_t * wait_set, rclcpp::Context::SharedPtr context)
-{
-  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 {
-    using rclcpp::contexts::default_context::get_global_default_context;
-    if (nullptr == context) {
-      context = get_global_default_context();
-    }
-    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, context->get_rcl_context().get(), options);
-    if (RCL_RET_OK != ret) {
-      rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize guard condition: ");
-    }
-    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*
-  }
-}
-
 bool
 rclcpp::sleep_for(const std::chrono::nanoseconds & nanoseconds, rclcpp::Context::SharedPtr context)
 {
-  std::chrono::nanoseconds time_left = nanoseconds;
-  {
-    std::unique_lock<std::mutex> lock(::g_interrupt_mutex);
-    auto start = std::chrono::steady_clock::now();
-    // this will release the lock while waiting
-    ::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;
+  if (nullptr == context) {
+    context = get_global_default_context();
   }
-  if (time_left > std::chrono::nanoseconds::zero() && ok(context)) {
-    return sleep_for(time_left);
-  }
-  // Return true if the timeout elapsed successfully, otherwise false.
-  return ok(context);
-}
-
-void
-rclcpp::notify_all()
-{
-  {
-    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_interrupt_condition_variable.notify_all();
+  return context->sleep_for(nanoseconds);
 }
 
 const char *

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

@@ -112,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);
 
@@ -131,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);

rclcpp_action/CHANGELOG.rst

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

rclcpp_action/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_action</name>
-  <version>0.6.1</version>
+  <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>

rclcpp_action/src/server_goal_handle.cpp

@@ -111,30 +111,19 @@ bool
 ServerGoalHandleBase::try_canceling() noexcept
 {
   std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
-  // Check if the goal reached a terminal state already
-  const bool active = rcl_action_goal_handle_is_active(rcl_handle_.get());
-  if (!active) {
-    return false;
-  }
-
   rcl_ret_t ret;
-
-  // Get the current state
-  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
-  ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
-  if (RCL_RET_OK != ret) {
-    return false;
-  }
-
-  // If it's not already canceling then transition to that state
-  if (GOAL_STATE_CANCELING != state) {
+  // 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;
     }
   }
 
-  // Get the state again
+  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;

rclcpp_lifecycle/CHANGELOG.rst

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

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -97,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(
@@ -107,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();

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

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -66,7 +66,8 @@ LifecycleNode::LifecycleNode(
   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())),
@@ -88,7 +89,8 @@ LifecycleNode::LifecycleNode(
       node_clock_,
       initial_parameters,
       use_intra_process_comms,
-      start_parameter_services
+      start_parameter_services,
+      allow_undeclared_parameters
     )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
   use_intra_process_comms_(use_intra_process_comms),