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,69 @@ 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 unique pointer is valid.
+   * The returned unique pointer can be promoted to a shared version.
+   *
+   * Releasing the returned unique pointer unregisters the callback.
+   * `remove_on_set_parameters_callback` can also be used.
+   *
+   * All the registered callbacks are called when a parameter is set.
+   * The order of the callbacks is not important.
+   *
+   * \param callback The callback to register.
+   * \returns A unique pointer. The callback is valid as long as the unique pointer is alive.
+   * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails.
+   */
+  RCLCPP_PUBLIC
+  OnSetParametersCallbackHandle::UniquePtr
+  add_on_set_parameters_callback(OnParametersSetCallbackType callback);
+
+  /// Remove a callback registered with `add_on_set_parameters_callback`.
+  /**
+   * Releases a handler, returned by `add_on_set_parameters_callback`.
+   *
+   * e.g.:
+   *
+   *    `remove_on_set_parameters_callback(scoped_callback.get())`
+   *
+   * As an alternative, the unique pointer can be released:
+   *
+   *    `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 released is an error.
+   * Releasing the smart pointer after calling `remove_on_set_parameters_callback` is not a problem.
+   *
+   * All the registered callbacks are called when a parameter is set.
+   * The order of the callbacks is not important.
+   *
+   * \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 <set>
 #include <string>
 #include <vector>
 
@@ -75,7 +76,9 @@ class ParameterMutationRecursionGuard
 };
 
 /// Implementation of the NodeParameters part of the Node API.
-class NodeParameters : public NodeParametersInterface
+class NodeParameters
+  : public NodeParametersInterface,
+  public std::enable_shared_from_this<NodeParameters>
 {
 public:
   RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParameters)
@@ -157,6 +160,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::UniquePtr
+  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 +181,8 @@ class NodeParameters : public NodeParametersInterface
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const override;
 
+  using CallbacksContainerType = std::set<OnSetParametersCallbackHandle *>;
+
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
 
@@ -180,7 +193,9 @@ class NodeParameters : public NodeParametersInterface
   // declare_parameter, etc).  In those cases, this will be set to false.
   bool parameter_modification_enabled_{true};
 
-  OnParametersSetCallbackType on_parameters_set_callback_ = nullptr;
+  OnParametersSetCallbackType on_parameters_set_callback_;
+
+  CallbacksContainerType on_parameters_set_callback_set_;
 
   std::map<std::string, ParameterInfo> parameters_;
 

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,21 @@ namespace rclcpp
 namespace node_interfaces
 {
 
+struct OnSetParametersCallbackHandle
+{
+  using UniquePtr = std::unique_ptr<
+    OnSetParametersCallbackHandle, std::function<void (OnSetParametersCallbackHandle *)>
+  >;
+  using SharedPtr = std::shared_ptr<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 +174,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::UniquePtr
+  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::UniquePtr
+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

@@ -289,20 +289,43 @@ __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)
+  const CallbacksContainerType & callback_set,
+  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);
+  for (auto & handle : callback_set) {
+    result = handle->callback(parameters);
+    if (!result.successful) {
+      return result;
+    }
+  }
+  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,
+  const CallbacksContainerType & callback_set,
+  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_set, callback);
   if (!result.successful) {
     return result;
   }
@@ -333,7 +356,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,
+  const CallbacksContainerType & callback_set,
+  const OnParametersSetCallbackType & callback,
   rcl_interfaces::msg::ParameterEvent * parameter_event_out,
   bool ignore_override = false)
 {
@@ -354,7 +378,8 @@ __declare_parameter_common(
   auto result = __set_parameters_atomically_common(
     parameter_wrappers,
     parameter_infos,
-    on_set_parameters_callback);
+    callback_set,
+    callback);
 
   // Add declared parameters to storage.
   parameters_out[name] = parameter_infos.at(name);
@@ -396,6 +421,7 @@ NodeParameters::declare_parameter(
     parameter_descriptor,
     parameters_,
     parameter_overrides_,
+    on_parameters_set_callback_set_,
     on_parameters_set_callback_,
     &parameter_event,
     ignore_override);
@@ -532,7 +558,8 @@ 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.
+      {},  // callback_set is explicitly empty, so that it is called only once below.
+      nullptr,  // callback is explicitly null.
       &parameter_event_msg,
       true);
     if (!result.successful) {
@@ -588,6 +615,8 @@ 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_set_,
+    // this will get called once, with all the parameters to be set
     on_parameters_set_callback_);
 
   // If not successful, then stop here.
@@ -820,6 +849,49 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
   return result;
 }
 
+void
+NodeParameters::remove_on_set_parameters_callback(
+  const OnSetParametersCallbackHandle * const handler)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  if (0 ==
+    on_parameters_set_callback_set_.erase(
+      const_cast<OnSetParametersCallbackHandle *>(handler)))
+  {
+    throw std::runtime_error("Callback doesn't exist");
+  }
+}
+
+OnSetParametersCallbackHandle::UniquePtr
+NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callback)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  auto handler = std::make_unique<OnSetParametersCallbackHandle>();
+  handler->callback = callback;
+  NodeParameters::WeakPtr node_parameters_weak(shared_from_this());
+
+  // *INDENT-OFF*  // uncrustify false positive
+  OnSetParametersCallbackHandle::UniquePtr scoped_handler(
+    handler.release(),
+    [node_parameters_weak](OnSetParametersCallbackHandle * handler) noexcept {
+      auto node_parameters_shared = node_parameters_weak.lock();
+      if (node_parameters_shared) {
+        try {
+          node_parameters_shared->remove_on_set_parameters_callback(handler);
+        } catch(...) {}  // It could have been manually deleted before, ignore the exception.
+      }
+      delete handler;
+      handler = NULL;
+  });
+  // *INDENT-ON*
+  on_parameters_set_callback_set_.insert(scoped_handler.get());
+  return scoped_handler;
+}
+
 NodeParameters::OnParametersSetCallbackType
 NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback)
 {