Allow registering multiple on_parameters_set_callback

rclcpp/include/rclcpp/node.hpp

@@ -889,10 +889,12 @@ class Node : public std::enable_shared_from_this<Node>
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
 
+  using OnSetParametersCallbackHandle =
+    rclcpp::node_interfaces::OnSetParametersCallbackHandle;
   using OnParametersSetCallbackType =
     rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
 
-  /// Register a callback to be called anytime a parameter is about to be changed.
+  /// Add a callback for when parameters are being set.
   /**
    * The callback signature is designed to allow handling of any of the above
    * `set_parameter*` or `declare_parameter*` methods, and so it takes a const
@@ -936,17 +938,68 @@ class Node : public std::enable_shared_from_this<Node>
    * of the latter things,
    * rclcpp::exceptions::ParameterModifiedInCallbackException will be thrown.
    *
-   * There may only be one callback set at a time, so the previously set
-   * callback is returned when this method is used, or nullptr will be returned
-   * if no callback was previously set.
+   * The callback functions must remain valid as long as the
+   * returned smart pointer is valid.
+   * The returned smart pointer can be promoted to a shared version.
+   *
+   * Resetting or letting the smart pointer go out of scope unregisters the callback.
+   * `remove_on_set_parameters_callback` can also be used.
+   *
+   * The registered callbacks are called when a parameter is set.
+   * When a callback returns a not successful result, the remaining callbacks aren't called.
+   * The order of the callback is the reverse from the registration order.
+   *
+   * \param callback The callback to register.
+   * \returns A shared pointer. The callback is valid as long as the smart pointer is alive.
+   * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
+   */
+  RCLCPP_PUBLIC
+  OnSetParametersCallbackHandle::SharedPtr
+  add_on_set_parameters_callback(OnParametersSetCallbackType callback);
+
+  /// Remove a callback registered with `add_on_set_parameters_callback`.
+  /**
+   * Delete a handler returned by `add_on_set_parameters_callback`.
+   *
+   * e.g.:
+   *
+   *    `remove_on_set_parameters_callback(scoped_callback.get())`
+   *
+   * As an alternative, the smart pointer can be reset:
+   *
+   *    `scoped_callback.reset()`
+   *
+   * Supposing that `scoped_callback` was the only owner.
+   *
+   * Calling `remove_on_set_parameters_callback` more than once with the same handler,
+   * or calling it after the shared pointer has been reset is an error.
+   * Resetting or letting the smart pointer go out of scope after calling
+   * `remove_on_set_parameters_callback` is not a problem.
+   *
+   * \param handler The callback handler to remove.
+   * \throws std::runtime_error if the handler was not created with `add_on_set_parameters_callback`,
+   *   or if it has been removed before.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler);
+
+  /// Register a callback to be called anytime a parameter is about to be changed.
+  /**
+   * With this method, only one callback can be set at a time. The callback that was previously
+   * set by this method is returned or `nullptr` if no callback was previously set.
+   *
+   * The callbacks added with `add_on_set_parameters_callback` are stored in a different place.
+   * `remove_on_set_parameters_callback` can't be used with the callbacks registered with this
+   * method. For removing it, use `set_on_parameters_set_callback(nullptr)`.
    *
    * \param[in] callback The callback to be called when the value for a
    *   parameter is about to be set.
    * \return The previous callback that was registered, if there was one,
    *   otherwise nullptr.
    */
   RCLCPP_PUBLIC
-  rclcpp::Node::OnParametersSetCallbackType
+  OnParametersSetCallbackType
   set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback);
 
   /// Register the callback for parameter changes

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -17,6 +17,7 @@
 
 #include <map>
 #include <memory>
+#include <list>
 #include <string>
 #include <vector>
 
@@ -157,6 +158,14 @@ class NodeParameters : public NodeParametersInterface
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
+  RCLCPP_PUBLIC
+  OnSetParametersCallbackHandle::SharedPtr
+  add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
+
+  RCLCPP_PUBLIC
+  void
+  remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override;
+
   RCLCPP_PUBLIC
   OnParametersSetCallbackType
   set_on_parameters_set_callback(OnParametersSetCallbackType callback) override;
@@ -170,6 +179,8 @@ class NodeParameters : public NodeParametersInterface
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const override;
 
+  using CallbacksContainerType = std::list<OnSetParametersCallbackHandle::WeakPtr>;
+
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
 
@@ -182,6 +193,8 @@ class NodeParameters : public NodeParametersInterface
 
   OnParametersSetCallbackType on_parameters_set_callback_ = nullptr;
 
+  CallbacksContainerType on_parameters_set_callback_container_;
+
   std::map<std::string, ParameterInfo> parameters_;
 
   std::map<std::string, rclcpp::ParameterValue> parameter_overrides_;

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_
 
 #include <map>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -32,6 +33,18 @@ namespace rclcpp
 namespace node_interfaces
 {
 
+struct OnSetParametersCallbackHandle
+{
+  RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle)
+
+  using OnParametersSetCallbackType =
+    std::function<
+    rcl_interfaces::msg::SetParametersResult(
+      const std::vector<rclcpp::Parameter> &)>;
+
+  OnParametersSetCallbackType callback;
+};
+
 /// Pure virtual interface class for the NodeParameters part of the Node API.
 class NodeParametersInterface
 {
@@ -158,14 +171,29 @@ class NodeParametersInterface
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
 
-  using OnParametersSetCallbackType =
-    std::function<
-    rcl_interfaces::msg::SetParametersResult(const std::vector<rclcpp::Parameter> &)
-    >;
+  using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType;
 
   using ParametersCallbackFunction [[deprecated("use OnParametersSetCallbackType instead")]] =
     OnParametersSetCallbackType;
 
+  /// Add a callback for when parameters are being set.
+  /**
+   * \sa rclcpp::Node::add_on_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  OnSetParametersCallbackHandle::SharedPtr
+  add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0;
+
+  /// Remove a callback registered with `add_on_set_parameters_callback`.
+  /**
+   * \sa rclcpp::Node::remove_on_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0;
+
   /// Register a callback for when parameters are being set, return an existing one.
   /**
    * \sa rclcpp::Node::set_on_parameters_set_callback

rclcpp/src/rclcpp/node.cpp

@@ -326,6 +326,18 @@ Node::list_parameters(const std::vector<std::string> & prefixes, uint64_t depth)
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
+rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr
+Node::add_on_set_parameters_callback(OnParametersSetCallbackType callback)
+{
+  return node_parameters_->add_on_set_parameters_callback(callback);
+}
+
+void
+Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback)
+{
+  return node_parameters_->remove_on_set_parameters_callback(callback);
+}
+
 rclcpp::Node::OnParametersSetCallbackType
 Node::set_on_parameters_set_callback(rclcpp::Node::OnParametersSetCallbackType callback)
 {

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -28,6 +28,7 @@
 
 #include <cmath>
 #include <cstdlib>
+#include <functional>
 #include <limits>
 #include <map>
 #include <memory>
@@ -289,20 +290,50 @@ __check_parameters(
 
 using OnParametersSetCallbackType =
   rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
+using CallbacksContainerType =
+  rclcpp::node_interfaces::NodeParameters::CallbacksContainerType;
+using OnSetParametersCallbackHandle =
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle;
 
 RCLCPP_LOCAL
 rcl_interfaces::msg::SetParametersResult
-__set_parameters_atomically_common(
+__call_on_parameters_set_callbacks(
   const std::vector<rclcpp::Parameter> & parameters,
-  std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameter_infos,
-  OnParametersSetCallbackType on_set_parameters_callback)
+  CallbacksContainerType & callback_container,
+  const OnParametersSetCallbackType & callback)
 {
-  // Call the user callback to see if the new value(s) are allowed.
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
-  if (on_set_parameters_callback) {
-    result = on_set_parameters_callback(parameters);
+  auto it = callback_container.begin();
+  while (it != callback_container.end()) {
+    auto shared_handle = it->lock();
+    if (nullptr != shared_handle) {
+      result = shared_handle->callback(parameters);
+      if (!result.successful) {
+        return result;
+      }
+      it++;
+    } else {
+      it = callback_container.erase(it);
+    }
+  }
+  if (callback) {
+    result = callback(parameters);
   }
+  return result;
+}
+
+RCLCPP_LOCAL
+rcl_interfaces::msg::SetParametersResult
+__set_parameters_atomically_common(
+  const std::vector<rclcpp::Parameter> & parameters,
+  std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameter_infos,
+  CallbacksContainerType & callback_container,
+  const OnParametersSetCallbackType & callback)
+{
+  // Call the user callback to see if the new value(s) are allowed.
+  rcl_interfaces::msg::SetParametersResult result =
+    __call_on_parameters_set_callbacks(parameters, callback_container, callback);
   if (!result.successful) {
     return result;
   }
@@ -333,7 +364,8 @@ __declare_parameter_common(
   const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
   std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameters_out,
   const std::map<std::string, rclcpp::ParameterValue> & overrides,
-  OnParametersSetCallbackType on_set_parameters_callback,
+  CallbacksContainerType & callback_container,
+  const OnParametersSetCallbackType & callback,
   rcl_interfaces::msg::ParameterEvent * parameter_event_out,
   bool ignore_override = false)
 {
@@ -354,7 +386,8 @@ __declare_parameter_common(
   auto result = __set_parameters_atomically_common(
     parameter_wrappers,
     parameter_infos,
-    on_set_parameters_callback);
+    callback_container,
+    callback);
 
   // Add declared parameters to storage.
   parameters_out[name] = parameter_infos.at(name);
@@ -396,6 +429,7 @@ NodeParameters::declare_parameter(
     parameter_descriptor,
     parameters_,
     parameter_overrides_,
+    on_parameters_set_callback_container_,
     on_parameters_set_callback_,
     &parameter_event,
     ignore_override);
@@ -523,6 +557,7 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
   std::map<std::string, rclcpp::node_interfaces::ParameterInfo> staged_parameter_changes;
   rcl_interfaces::msg::ParameterEvent parameter_event_msg;
   parameter_event_msg.node = combined_name_;
+  CallbacksContainerType empty_callback_container;
   for (auto parameter_to_be_declared : parameters_to_be_declared) {
     // This should not throw, because we validated the name and checked that
     // the parameter was not already declared.
@@ -532,7 +567,9 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
       rcl_interfaces::msg::ParameterDescriptor(),  // Implicit declare uses default descriptor.
       staged_parameter_changes,
       parameter_overrides_,
-      nullptr,  // callback is explicitly null, so that it is called only once, when setting below.
+      // Only call callbacks once below
+      empty_callback_container,  // callback_container is explicitly empty
+      nullptr,  // callback is explicitly null.
       &parameter_event_msg,
       true);
     if (!result.successful) {
@@ -588,6 +625,9 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
     // they are actually set on the official parameter storage
     parameters_,
     // this will get called once, with all the parameters to be set
+    on_parameters_set_callback_container_,
+    // These callbacks are called once. When a callback returns an unsuccessful result,
+    // the remaining aren't called.
     on_parameters_set_callback_);
 
   // If not successful, then stop here.
@@ -820,6 +860,53 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
   return result;
 }
 
+struct HandleCompare
+  : public std::unary_function<OnSetParametersCallbackHandle::WeakPtr, bool>
+{
+  explicit HandleCompare(const OnSetParametersCallbackHandle * const base)
+  : base_(base) {}
+  bool operator()(const OnSetParametersCallbackHandle::WeakPtr & handle)
+  {
+    auto shared_handle = handle.lock();
+    if (base_ == shared_handle.get()) {
+      return true;
+    }
+    return false;
+  }
+  const OnSetParametersCallbackHandle * const base_;
+};
+
+void
+NodeParameters::remove_on_set_parameters_callback(
+  const OnSetParametersCallbackHandle * const handle)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  auto it = std::find_if(
+    on_parameters_set_callback_container_.begin(),
+    on_parameters_set_callback_container_.end(),
+    HandleCompare(handle));
+  if (it != on_parameters_set_callback_container_.end()) {
+    on_parameters_set_callback_container_.erase(it);
+  } else {
+    throw std::runtime_error("Callback doesn't exist");
+  }
+}
+
+OnSetParametersCallbackHandle::SharedPtr
+NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callback)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  auto handle = std::make_shared<OnSetParametersCallbackHandle>();
+  handle->callback = callback;
+  // the last callback registered is executed first.
+  on_parameters_set_callback_container_.emplace_front(handle);
+  return handle;
+}
+
 NodeParameters::OnParametersSetCallbackType
 NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback)
 {