use constants from rcl for parameter names

Signed-off-by: Brian Chen <brian.chen@openrobotics.org>

rclcpp/CMakeLists.txt

@@ -65,6 +65,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/guard_condition.cpp
   src/rclcpp/init_options.cpp
   src/rclcpp/intra_process_manager.cpp
+  src/rclcpp/introspection.cpp
   src/rclcpp/logger.cpp
   src/rclcpp/logging_mutex.cpp
   src/rclcpp/memory_strategies.cpp

rclcpp/doc/proposed_node_parameter_callbacks.md

@@ -0,0 +1,33 @@
+# Proposed node parameters callback Design
+
+## Introduction:
+
+The original requirement came in **gazeb_ros_pkgs** for setting individual wheel slip params based on global wheel slip value [link to original issue](https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1365). 
+The main idea was to set some parameters using `set_parameter` API once a given param was set successfully. 
+The requirement let to the discussion of supporting registered callbacks once the parameters are set and have been requested by users before.
+
+In the current Node API, the `add_on_set_parameters_callback` is used for doing any validation for parameters before setting the parameters successfully and once the parameters are validated `ParameterEventHandler` object is used to publish any 
+changes to node parameters on **/parameter_events** topic, which can be subscribed by any node to see the changed parameter(Note that the **/parameter_events** is a topic for all nodes on the network, and we have to rely on executors to process those).
+
+We propose adding a `PostSetParametersCallbackHandle` for successful parameter set similar to OnSetParametersCallbackHandle for parameter validation, 
+additionally we propose adding a `PreSetParametersCallbackHandle` which can be used to modify the parameters being set
+
+Some related discussion here [#609](https://github.com/ros2/rclcpp/issues/609) [#1789](https://github.com/ros2/rclcpp/pull/1789).
+
+## Proposed Architecture
+
+The validation callback is often abused to trigger side effects in the code, for instance updating class attributes even before parameter has been set successfully. 
+Instead of relying on **/parameter_events** it would be good to support an internal node interface API for registering callbacks for successful parameter set 
+`add_post_set_parameters_callback`.
+
+We can use the proposed `add_post_set_parameters_callback` for setting some param but this might result in unpredictable behaviour because of infinite recursion 
+and due to the params being set atomically. In order to get around this we propose adding another registered callback `add_pre_set_parameters_callback` which 
+will be triggered before the validation callbacks and can be used to modify the parameter list as required.
+
+![Desgin API](https://github.com/ros2/rclcpp/blob/deepanshu/local-param-changed-callback-support/rclcpp/doc/param_callback_design.png?raw=true)
+
+## Alternatives
+
+* The **/parameter_events** topic can be used to monitor any changes to node parameters and use ```set_parameter``` API if required to set any further parameters. However, it seems weird that we have to rely on network to notify nodes of their own parameter changes. Therefore, it makes sense to support a callback which gets triggered after the parameter is changed, so that it can trigger some more events.
+* The ```add_on_set_parameters_callback``` can be used to ```set_paramters```, however the ```ParameterMutationRecursionGuard``` will not allow this since the ```add_on_set_parameters_callback``` is supposed to be used for validation purposes only.
+* The ```add_post_set_parameters_callback```  can be used to set parameters but this might result in different behaviours depending on whether the params were set atomically or not. Also, we might enter an infinite recursion depending on how params are being set. 

rclcpp/include/rclcpp/client.hpp

@@ -17,6 +17,7 @@
 
 #include <atomic>
 #include <future>
+#include <functional>
 #include <unordered_map>
 #include <memory>
 #include <mutex>

rclcpp/include/rclcpp/create_client.hpp

@@ -5,7 +5,6 @@
 // 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.
@@ -15,10 +14,12 @@
 #ifndef RCLCPP__CREATE_CLIENT_HPP_
 #define RCLCPP__CREATE_CLIENT_HPP_
 
+#include <iostream>
 #include <memory>
 #include <string>
 
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rmw/rmw.h"
 
@@ -33,12 +34,19 @@ create_client(
   std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
   std::shared_ptr<node_interfaces::NodeGraphInterface> node_graph,
   std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+  std::shared_ptr<node_interfaces::NodeClockInterface> node_clock,
   const std::string & service_name,
   const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
+  rclcpp::CallbackGroup::SharedPtr group,
+  bool enable_service_introspection)
 {
+
   rcl_client_options_t options = rcl_client_get_default_options();
   options.qos = qos_profile;
+  if (enable_service_introspection) {
+    options.enable_service_introspection = enable_service_introspection;
+    options.clock = node_clock->get_clock()->get_clock_handle();
+  }
 
   auto cli = rclcpp::Client<ServiceT>::make_shared(
     node_base.get(),

rclcpp/include/rclcpp/create_service.hpp

@@ -20,6 +20,7 @@
 #include <utility>
 
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 #include "rclcpp/node_interfaces/node_services_interface.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rmw/rmw.h"
@@ -27,23 +28,31 @@
 namespace rclcpp
 {
 
-/// Create a service with a given type.
+
+/// Create a service with a given type
 /// \internal
 template<typename ServiceT, typename CallbackT>
 typename rclcpp::Service<ServiceT>::SharedPtr
 create_service(
-  std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
-  std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
-  const std::string & service_name,
-  CallbackT && callback,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::CallbackGroup::SharedPtr group)
+    std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
+    std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+    std::shared_ptr<node_interfaces::NodeClockInterface> node_clock,
+    const std::string & service_name,
+    CallbackT && callback,
+    const rmw_qos_profile_t & qos_profile,
+    rclcpp::CallbackGroup::SharedPtr group,
+    bool enable_service_introspection
+    )
 {
   rclcpp::AnyServiceCallback<ServiceT> any_service_callback;
   any_service_callback.set(std::forward<CallbackT>(callback));
 
   rcl_service_options_t service_options = rcl_service_get_default_options();
   service_options.qos = qos_profile;
+  if (enable_service_introspection) {
+    service_options.enable_service_introspection = enable_service_introspection;
+    service_options.clock = node_clock->get_clock()->get_clock_handle();
+  }
 
   auto serv = Service<ServiceT>::make_shared(
     node_base->get_shared_rcl_node_handle(),
@@ -53,6 +62,7 @@ create_service(
   return serv;
 }
 
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__CREATE_SERVICE_HPP_

rclcpp/include/rclcpp/introspection.hpp

@@ -0,0 +1,69 @@
+// Copyright 2022 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__INTROSPECTION_HPP_
+#define RCLCPP__INTROSPECTION_HPP_
+
+#include "rclcpp/client.hpp"
+#include "rclcpp/node_interfaces/node_base.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/parameter_value.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/service.hpp" // this smells circular
+#include "rcl/node.h"
+#include "rcl/service.h"
+#include "rcl/client.h"
+#include "rcl/introspection.h"
+
+namespace rclcpp
+{
+
+/* Provides on-parameter-change configuration features to service introspection
+ *
+ *
+ */
+class IntrospectionUtils
+{
+  public:
+
+    explicit IntrospectionUtils(
+        rcl_node_t * rcl_node_ptr,
+        const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr& node_parameters);
+    
+    explicit IntrospectionUtils(
+        const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr & node_base,
+        const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr& node_parameters)
+      : IntrospectionUtils(node_base->get_rcl_node_handle(), node_parameters){};
+
+    virtual ~IntrospectionUtils();
+    void register_service(const rclcpp::ServiceBase::SharedPtr& service);
+    void register_client(const rclcpp::ClientBase::SharedPtr& client);
+
+
+    
+  private:
+    std::vector<rcl_service_t *> services;
+    std::vector<rcl_client_t *> clients;
+    rcl_node_t * rcl_node_ptr_;
+    std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> node_parameters_;
+};
+  
+
+
+
+
+
+} // namespace rclcpp
+#endif // RCLCPP__INTROSPECTION_HPP_

rclcpp/include/rclcpp/node.hpp

@@ -69,6 +69,7 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/timer.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rclcpp/introspection.hpp"
 
 namespace rclcpp
 {
@@ -268,7 +269,8 @@ public:
     const std::string & service_name,
     CallbackT && callback,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default,
-    rclcpp::CallbackGroup::SharedPtr group = nullptr);
+    rclcpp::CallbackGroup::SharedPtr group = nullptr
+    );
 
   /// Create and return a GenericPublisher.
   /**
@@ -859,10 +861,77 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const;
 
+  using PreSetParametersCallbackHandle =
+      rclcpp::node_interfaces::PreSetParametersCallbackHandle;
+  using PreSetParametersCallbackType =
+      rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType;
+
   using OnSetParametersCallbackHandle =
     rclcpp::node_interfaces::OnSetParametersCallbackHandle;
-  using OnParametersSetCallbackType =
-    rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
+  using OnSetParametersCallbackType =
+    rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType;
+
+  using PostSetParametersCallbackHandle =
+      rclcpp::node_interfaces::PostSetParametersCallbackHandle;
+  using PostSetParametersCallbackType =
+      rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType;
+
+  /// Add a callback gets triggered before parameters are validated.
+  /**
+   * This callback can be used to modify the original list of parameters being
+   * set by the user. The modified list of parameters is then forwarded to the
+   * "on set parameter" callback for validation.
+   *
+   * The callback signature is designed to allow handling of any of the `set_parameter*`
+   * methods. The callback takes a reference to a vector of parameters to be set.
+   * This vector of parameters can further be modified based on the user requirement.
+   *
+   * One of the use case of "pre set callback" can be updating additional parameters
+   * conditioned on changes to a parameter.
+   *
+   * For an example callback:
+   *
+   *  void preSetParameterCallback(std::vector<rclcpp::Parameter>& parameters){
+   *  for(auto&param:parameters){
+   *     // if "param1" is being set try setting "param2" as well.
+   *    if(param.get_name() == "param1"){
+   *      auto newParam = rclcpp::Parameter("param2", 4.0);
+   *      auto it = std::find(parameters.begin(), parameters.end(), newParam);
+   *      if(it == parameters.end()){
+   *        parameters.push_back(newParam);
+   *      }else{
+   *        *it = newParam;
+   *      }
+   *    }
+   *  }
+   *};
+   *
+   * The above callback takes list of params by reference and appends 'param2' to the modified
+   * list of params based on the condition that 'param1' is being set by the user. Further,
+   * before appending 'param2' to the modified list of params the callback checks if 'param2'
+   * is already present in the list of params being set.
+   *
+   * Note that once the vector of parameters is modified the pre set parameter callback
+   * will set the returned parameter list atomically. This makes sense since the change of
+   * one parameter is conditioned on some other parameter.
+   *
+   * Also note that the callback is only called while setting parameters(set_parameter,
+   * set_parameters, set_parameters_atomically) and is not called while the parameters
+   * are being declared using declare_parameters or it's variants.
+   *
+   * An empty modified parameter list from the callback will result in "set_parameter*"
+   * returning an unsuccessful result.
+   *
+   * The 'remove_pre_set_parameters_callback' can be used to deregister the callback.
+   *
+   * \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 PreSetParametersCallbackHandle fails.
+   */
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PreSetParametersCallbackHandle::SharedPtr
+  add_pre_set_parameters_callback(PreSetParametersCallbackType callback);
 
   /// Add a callback for when parameters are being set.
   /**
@@ -931,7 +1000,67 @@ public:
   RCLCPP_PUBLIC
   RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
-  add_on_set_parameters_callback(OnParametersSetCallbackType callback);
+  add_on_set_parameters_callback(OnSetParametersCallbackType callback);
+
+  /// Add a callback gets triggered after parameters are set successfully.
+  /**
+   * This callback gets triggered after the parameters have been set successfully
+   * The callback gets called after successful validation of parameters after
+   * the "on set parameter" registered callback.    *
+   *
+   * The callback signature is designed to allow handling of any of the `set_parameter*`
+   * or `declare_parameter` methods. The callback takes a reference to a const vector of
+   * parameters that have been set successfully.
+   *
+   * The post callback can be valuable as a place to cause side-effects based on parameter
+   * changes. For instance updating the internally tracked class attributes once the params
+   * have been changed successfully.
+   *
+   * For an example callback:
+   *
+   * void postSetParameterCallback(const std::vector<rclcpp::Parameter>& parameters){
+   *  for(const auto&param:parameters){
+   *   // the internal class member can be changed after
+   *   // successful change to param1 or param2
+   *    if(param.get_name() == "param1"){
+   *      internal_tracked_class_parameter_1_ = param.get_value<double>();
+   *    }
+   *    else if(param.get_name() == "param2"){
+   *      internal_tracked_class_parameter_2_ = param.get_value<double>();
+   *    }
+   *  }
+   *};
+   *
+   * The above callback takes a const reference to list of parameters that have been
+   * set successfully and as a result of this updates the internally tracked class attributes
+   * "internal_tracked_class_parameter_1_" and "internal_tracked_class_parameter_2_"
+   * respectively.
+   *
+   * Note that this callback should not be used to request changes to parameters based on
+   * another and instead "pre set parameter" callback should be used for such usages.
+   *
+   * The 'remove_post_set_parameters_callback' can be used to deregister the callback.
+   *
+   * \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
+  RCUTILS_WARN_UNUSED
+  PostSetParametersCallbackHandle::SharedPtr
+  add_post_set_parameters_callback(PostSetParametersCallbackType callback);
+
+  /// Remove a callback registered with `add_pre_set_parameters_callback`.
+  /**
+   * Delete a handler returned by `add_pre_set_parameters_callback`.
+   *
+   * \param handler The callback handler to remove.
+   * \throws std::runtime_error if the handler was not created with `add_pre_set_parameters_callback`,
+   *   or if it has been removed before.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler);
 
   /// Remove a callback registered with `add_on_set_parameters_callback`.
   /**
@@ -960,6 +1089,18 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler);
 
+  /// Remove a callback registered with `add_post_set_parameters_callback`.
+  /**
+   * Delete a handler returned by `add_post_set_parameters_callback`.
+   *
+   * \param handler The callback handler to remove.
+   * \throws std::runtime_error if the handler was not created with `add_post_set_parameters_callback`,
+   *   or if it has been removed before.
+   */
+  RCLCPP_PUBLIC
+  void
+  remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler);
+
   /// Get the fully-qualified names of all available nodes.
   /**
    * The fully-qualified name includes the local namespace and name of the node.
@@ -1309,6 +1450,8 @@ private:
   rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_;
   rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
 
+  std::shared_ptr<rclcpp::IntrospectionUtils> introspection_utils_;
+
   const rclcpp::NodeOptions node_options_;
   const std::string sub_namespace_;
   const std::string effective_namespace_;

rclcpp/include/rclcpp/node_impl.hpp

@@ -127,13 +127,19 @@ Node::create_client(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::CallbackGroup::SharedPtr group)
 {
-  return rclcpp::create_client<ServiceT>(
+  typename Client<ServiceT>::SharedPtr cli = rclcpp::create_client<ServiceT>(
     node_base_,
     node_graph_,
     node_services_,
+    node_clock_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     qos_profile,
-    group);
+    group,
+    node_options_.enable_service_introspection()
+    );
+
+  introspection_utils_->register_client(cli);
+  return cli;
 }
 
 template<typename ServiceT, typename CallbackT>
@@ -144,13 +150,18 @@ Node::create_service(
   const rmw_qos_profile_t & qos_profile,
   rclcpp::CallbackGroup::SharedPtr group)
 {
-  return rclcpp::create_service<ServiceT, CallbackT>(
+  typename rclcpp::Service<ServiceT>::SharedPtr serv = rclcpp::create_service<ServiceT, CallbackT>(
     node_base_,
     node_services_,
+    node_clock_,
     extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     std::forward<CallbackT>(callback),
     qos_profile,
-    group);
+    group,
+    node_options_.enable_service_introspection()
+    );
+  introspection_utils_->register_service(serv);
+  return serv;
 }
 
 template<typename AllocatorT>

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -107,7 +107,8 @@ public:
     const rclcpp::QoS & parameter_event_qos,
     const rclcpp::PublisherOptionsBase & parameter_event_publisher_options,
     bool allow_undeclared_parameters,
-    bool automatically_declare_parameters_from_overrides);
+    bool automatically_declare_parameters_from_overrides,
+    bool enable_service_introspection_for_parameter_service);
 
   RCLCPP_PUBLIC
   virtual
@@ -181,20 +182,40 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const override;
 
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PreSetParametersCallbackHandle::SharedPtr
+  add_pre_set_parameters_callback(PreSetParametersCallbackType callback) override;
+
   RCLCPP_PUBLIC
   RCUTILS_WARN_UNUSED
   OnSetParametersCallbackHandle::SharedPtr
-  add_on_set_parameters_callback(OnParametersSetCallbackType callback) override;
+  add_on_set_parameters_callback(OnSetParametersCallbackType callback) override;
+
+  RCLCPP_PUBLIC
+  RCUTILS_WARN_UNUSED
+  PostSetParametersCallbackHandle::SharedPtr
+  add_post_set_parameters_callback(PostSetParametersCallbackType callback) override;
 
   RCLCPP_PUBLIC
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override;
 
+  RCLCPP_PUBLIC
+  void
+  remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) override;
+
+  RCLCPP_PUBLIC
+  void
+  remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) override;
+
   RCLCPP_PUBLIC
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const override;
 
-  using CallbacksContainerType = std::list<OnSetParametersCallbackHandle::WeakPtr>;
+  using PreSetCallbacksHandleContainer = std::list<PreSetParametersCallbackHandle::WeakPtr>;
+  using OnSetCallbacksHandleContainer = std::list<OnSetParametersCallbackHandle::WeakPtr>;
+  using PostSetCallbacksHandleContainer = std::list<PostSetParametersCallbackHandle::WeakPtr>;
 
 protected:
   RCLCPP_PUBLIC
@@ -211,7 +232,11 @@ private:
   // declare_parameter, etc).  In those cases, this will be set to false.
   bool parameter_modification_enabled_{true};
 
-  CallbacksContainerType on_parameters_set_callback_container_;
+  PreSetCallbacksHandleContainer pre_set_parameter_callback_container_;
+
+  OnSetCallbacksHandleContainer on_set_parameters_callback_container_;
+
+  PostSetCallbacksHandleContainer post_set_parameter_callback_container_;
 
   std::map<std::string, ParameterInfo> parameters_;
 

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 <functional>
 #include <map>
 #include <memory>
 #include <string>
@@ -33,18 +34,40 @@ namespace rclcpp
 namespace node_interfaces
 {
 
+struct PreSetParametersCallbackHandle
+{
+  RCLCPP_SMART_PTR_DEFINITIONS(PreSetParametersCallbackHandle)
+
+  using PreSetParametersCallbackType =
+    std::function<void(std::vector<rclcpp::Parameter> &)>;
+
+  PreSetParametersCallbackType callback;
+};
+
 struct OnSetParametersCallbackHandle
 {
   RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle)
 
-  using OnParametersSetCallbackType =
+  using OnSetParametersCallbackType =
     std::function<
     rcl_interfaces::msg::SetParametersResult(
       const std::vector<rclcpp::Parameter> &)>;
 
-  OnParametersSetCallbackType callback;
+  OnSetParametersCallbackType callback;
 };
 
+// parameter callback gets invoked after successful node parameter set.
+struct PostSetParametersCallbackHandle
+{
+  RCLCPP_SMART_PTR_DEFINITIONS(PostSetParametersCallbackHandle)
+
+  using PostSetParametersCallbackType =
+    std::function<void(const std::vector<rclcpp::Parameter> &)>;
+
+  PostSetParametersCallbackType callback;
+};
+
+
 /// Pure virtual interface class for the NodeParameters part of the Node API.
 class NodeParametersInterface
 {
@@ -110,15 +133,11 @@ public:
   std::vector<rcl_interfaces::msg::SetParametersResult>
   set_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;
 
-  /// Set one or more parameters, all at once.
-  /**
-   * \sa rclcpp::Node::set_parameters_atomically
-   */
   RCLCPP_PUBLIC
   virtual
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(
-    const std::vector<rclcpp::Parameter> & parameters) = 0;
+      const std::vector<rclcpp::Parameter> & parameters) = 0;
 
   /// Get descriptions of parameters given their names.
   /*
@@ -185,7 +204,18 @@ public:
   rcl_interfaces::msg::ListParametersResult
   list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const = 0;
 
-  using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType;
+  using OnSetParametersCallbackType = OnSetParametersCallbackHandle::OnSetParametersCallbackType;
+  using PostSetParametersCallbackType = PostSetParametersCallbackHandle::PostSetParametersCallbackType;
+  using PreSetParametersCallbackType = PreSetParametersCallbackHandle::PreSetParametersCallbackType;
+
+  /// Add a callback gets triggered before parameters are validated.
+  /**
+   * \sa rclcpp::Node::add_pre_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  PreSetParametersCallbackHandle::SharedPtr
+  add_pre_set_parameters_callback(PreSetParametersCallbackType callback) = 0;
 
   /// Add a callback for when parameters are being set.
   /**
@@ -194,7 +224,25 @@ public:
   RCLCPP_PUBLIC
   virtual
   OnSetParametersCallbackHandle::SharedPtr
-  add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0;
+  add_on_set_parameters_callback(OnSetParametersCallbackType callback) = 0;
+
+  /// Add a callback gets triggered after parameters are set successfully.
+  /**
+   * \sa rclcpp::Node::add_post_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+      PostSetParametersCallbackHandle::SharedPtr
+  add_post_set_parameters_callback(PostSetParametersCallbackType callback) = 0;
+
+  /// Remove a callback registered with `add_pre_set_parameters_callback`.
+  /**
+   * \sa rclcpp::Node::remove_pre_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) = 0;
 
   /// Remove a callback registered with `add_on_set_parameters_callback`.
   /**
@@ -205,6 +253,15 @@ public:
   void
   remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0;
 
+  /// Remove a callback registered with `add_post_set_parameters_callback`.
+  /**
+   * \sa rclcpp::Node::remove_post_set_parameters_callback
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) = 0;
+
   /// Return the initial parameter values used by the NodeParameters to override default values.
   RCLCPP_PUBLIC
   virtual

rclcpp/include/rclcpp/node_options.hpp

@@ -44,6 +44,7 @@ public:
    *   - use_global_arguments = true
    *   - use_intra_process_comms = false
    *   - enable_topic_statistics = false
+   *   - enable_service_introspection = false
    *   - start_parameter_services = true
    *   - start_parameter_event_publisher = true
    *   - clock_qos = rclcpp::ClockQoS()
@@ -199,6 +200,16 @@ public:
   bool
   enable_topic_statistics() const;
 
+  /// Return the enable_service_introspection flag
+  RCLCPP_PUBLIC
+  bool
+  enable_service_introspection() const;
+
+  /// Set the enable_service_introspection flag
+  RCLCPP_PUBLIC
+  NodeOptions &
+  enable_service_introspection(bool enable_service_introspection);
+
   /// Set the enable_topic_statistics flag, return this for parameter idiom.
   /**
    * If true, topic statistics collection and publication will be enabled
@@ -396,6 +407,8 @@ private:
 
   bool enable_topic_statistics_ {false};
 
+  bool enable_service_introspection_ {false};
+
   bool start_parameter_services_ {true};
 
   bool start_parameter_event_publisher_ {true};

rclcpp/include/rclcpp/parameter_service.hpp

@@ -43,7 +43,9 @@ public:
   explicit ParameterService(
     const std::shared_ptr<node_interfaces::NodeBaseInterface> node_base,
     const std::shared_ptr<node_interfaces::NodeServicesInterface> node_services,
+    const std::shared_ptr<node_interfaces::NodeClockInterface> node_clock,
     rclcpp::node_interfaces::NodeParametersInterface * node_params,
+    bool enable_service_introspection = false,
     const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters);
 
 private:

rclcpp/src/rclcpp/introspection.cpp

@@ -0,0 +1,93 @@
+#include "rclcpp/introspection.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rcl/introspection.h"
+
+
+using rclcpp::IntrospectionUtils;
+
+IntrospectionUtils::IntrospectionUtils(
+        rcl_node_t * rcl_node_ptr,
+        const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr& node_parameters)
+: rcl_node_ptr_(rcl_node_ptr),
+  node_parameters_(node_parameters)
+{
+
+// TODO(ihasdapie): Use parameter #defines in rcl/introspection.h
+
+  // declare service introspection parameters
+  if (!node_parameters_->has_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER)) {
+    node_parameters_->declare_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER,
+        rclcpp::ParameterValue(true));
+  }
+  if (!node_parameters_->has_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_EVENT_CONTENT_PARAMETER)) {
+    node_parameters_->declare_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_EVENT_CONTENT_PARAMETER,
+        rclcpp::ParameterValue(true));
+  }
+  if (!node_parameters_->has_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER)) {
+    node_parameters_->declare_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER,
+        rclcpp::ParameterValue(true));
+  }
+  if (!node_parameters_->has_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_EVENT_CONTENT_PARAMETER)) {
+    node_parameters_->declare_parameter(RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_EVENT_CONTENT_PARAMETER,
+        rclcpp::ParameterValue(true));
+  }
+
+  std::function<void(const std::vector<rclcpp::Parameter> &)>
+    configure_service_introspection_callback = 
+    [this](const std::vector<rclcpp::Parameter> & parameters) {
+      rcl_ret_t ret;
+      for (const auto & param: parameters) {
+        if (param.get_name() == RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_PARAMETER) {
+          for (rcl_service_t * srv: services) {
+            ret = rcl_service_introspection_configure_service_events(
+                srv, this->rcl_node_ptr_, param.get_value<bool>());
+            if (RCL_RET_OK != ret) {
+              throw std::runtime_error("Could not configure service introspection events");
+            }
+          }
+        } else if (param.get_name() == RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_PARAMETER) {
+          for (rcl_client_t * clt: clients) {
+            ret = rcl_service_introspection_configure_client_events(
+                clt, this->rcl_node_ptr_, param.get_value<bool>());
+            if (RCL_RET_OK != ret) {
+              throw std::runtime_error("Could not configure client introspection events");
+            }
+          }
+        } else if (param.get_name() == RCL_SERVICE_INTROSPECTION_PUBLISH_SERVICE_EVENT_CONTENT_PARAMETER) {
+          for (rcl_service_t * srv: services) {
+            rcl_service_introspection_configure_service_content(srv, param.get_value<bool>());
+          }
+        } else if (param.get_name() == RCL_SERVICE_INTROSPECTION_PUBLISH_CLIENT_EVENT_CONTENT_PARAMETER) {
+          for (rcl_client_t * clt: clients) {
+            rcl_service_introspection_configure_client_content(clt, param.get_value<bool>());
+          }
+        }
+      }
+    };
+
+  // register callbacks
+  node_parameters_->add_post_set_parameters_callback(configure_service_introspection_callback);
+}
+
+// IntrospectionUtils::~IntrospectionUtils();
+
+
+// Alternatively this wrapper can be made to wrap a create_client call?
+
+void IntrospectionUtils::register_service(
+    const rclcpp::ServiceBase::SharedPtr& service){
+  this->services.push_back(service->get_service_handle().get());
+}
+
+void IntrospectionUtils::register_client(
+    const rclcpp::ClientBase::SharedPtr& client){
+  this->clients.push_back(client->get_client_handle().get());
+}
+
+
+    
+
+IntrospectionUtils::~IntrospectionUtils() = default;
+
+
+

rclcpp/src/rclcpp/node.cpp

@@ -26,6 +26,7 @@
 #include "rclcpp/detail/qos_parameters.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/graph_listener.hpp"
+#include "rclcpp/introspection.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_base.hpp"
 #include "rclcpp/node_interfaces/node_clock.hpp"
@@ -192,7 +193,8 @@ Node::Node(
       get_parameter_events_qos(*node_base_, options),
       options.parameter_event_publisher_options(),
       options.allow_undeclared_parameters(),
-      options.automatically_declare_parameters_from_overrides()
+      options.automatically_declare_parameters_from_overrides(),
+      options.enable_service_introspection()
     )),
   node_time_source_(new rclcpp::node_interfaces::NodeTimeSource(
       node_base_,
@@ -206,6 +208,10 @@ Node::Node(
       options.use_clock_thread()
     )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
+  introspection_utils_(new rclcpp::IntrospectionUtils(
+        node_base_,
+        node_parameters_
+        )),
   node_options_(options),
   sub_namespace_(""),
   effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_))
@@ -353,7 +359,7 @@ Node::has_parameter(const std::string & name) const
 rcl_interfaces::msg::SetParametersResult
 Node::set_parameter(const rclcpp::Parameter & parameter)
 {
-  return this->set_parameters_atomically({parameter});
+  return node_parameters_->set_parameters_atomically({parameter});
 }
 
 std::vector<rcl_interfaces::msg::SetParametersResult>
@@ -418,16 +424,38 @@ Node::list_parameters(const std::vector<std::string> & prefixes, uint64_t depth)
   return node_parameters_->list_parameters(prefixes, depth);
 }
 
+rclcpp::Node::PreSetParametersCallbackHandle::SharedPtr
+Node::add_pre_set_parameters_callback(PreSetParametersCallbackType callback){
+  return node_parameters_->add_pre_set_parameters_callback(callback);
+}
+
 rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr
-Node::add_on_set_parameters_callback(OnParametersSetCallbackType callback)
+Node::add_on_set_parameters_callback(OnSetParametersCallbackType callback)
 {
   return node_parameters_->add_on_set_parameters_callback(callback);
 }
 
+rclcpp::Node::PostSetParametersCallbackHandle::SharedPtr
+Node::add_post_set_parameters_callback(PostSetParametersCallbackType callback){
+  return node_parameters_->add_post_set_parameters_callback(callback);
+}
+
 void
-Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback)
+Node::remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler)
 {
-  return node_parameters_->remove_on_set_parameters_callback(callback);
+  return node_parameters_->remove_pre_set_parameters_callback(handler);
+}
+
+void
+Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler)
+{
+  return node_parameters_->remove_on_set_parameters_callback(handler);
+}
+
+void
+Node::remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler)
+{
+  return node_parameters_->remove_post_set_parameters_callback(handler);
 }
 
 std::vector<std::string>

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -76,7 +76,8 @@ NodeParameters::NodeParameters(
   const rclcpp::QoS & parameter_event_qos,
   const rclcpp::PublisherOptionsBase & parameter_event_publisher_options,
   bool allow_undeclared_parameters,
-  bool automatically_declare_parameters_from_overrides)
+  bool automatically_declare_parameters_from_overrides,
+  bool enable_service_introspection_for_parameter_service)
 : allow_undeclared_(allow_undeclared_parameters),
   events_publisher_(nullptr),
   node_logging_(node_logging),
@@ -91,7 +92,8 @@ NodeParameters::NodeParameters(
   publisher_options.allocator = std::make_shared<AllocatorT>();
 
   if (start_parameter_services) {
-    parameter_service_ = std::make_shared<ParameterService>(node_base, node_services, this);
+    parameter_service_ = std::make_shared<ParameterService>(node_base, node_services, node_clock,
+        this, enable_service_introspection_for_parameter_service);
   }
 
   if (start_parameter_event_publisher) {
@@ -305,18 +307,55 @@ __check_parameters(
   return result;
 }
 
-using OnParametersSetCallbackType =
-  rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType;
-using CallbacksContainerType =
-  rclcpp::node_interfaces::NodeParameters::CallbacksContainerType;
+using PreSetParametersCallbackType =
+    rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType;
+using PreSetParametersCallbackHandle =
+    rclcpp::node_interfaces::PreSetParametersCallbackHandle;
+using PreSetCallbacksHandleContainer =
+    rclcpp::node_interfaces::NodeParameters::PreSetCallbacksHandleContainer;
+
+using OnSetParametersCallbackType =
+  rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType;
 using OnSetParametersCallbackHandle =
   rclcpp::node_interfaces::OnSetParametersCallbackHandle;
+using OnSetCallbacksHandleContainer =
+    rclcpp::node_interfaces::NodeParameters::OnSetCallbacksHandleContainer;
+
+using PostSetParametersCallbackType =
+    rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType;
+using PostSetParametersCallbackHandle =
+    rclcpp::node_interfaces::PostSetParametersCallbackHandle;
+using PostSetCallbacksHandleContainer =
+    rclcpp::node_interfaces::NodeParameters::PostSetCallbacksHandleContainer;
+
+RCLCPP_LOCAL
+bool
+__call_pre_set_parameters_callbacks(
+    std::vector<rclcpp::Parameter> & parameters,
+    PreSetCallbacksHandleContainer & callback_container)
+{
+  if(!callback_container.empty()){
+    auto it = callback_container.begin();
+    while (it != callback_container.end()) {
+      auto shared_handle = it->lock();
+      if (nullptr != shared_handle) {
+        shared_handle->callback(parameters);
+        it++;
+      } else {
+        it = callback_container.erase(it);
+      }
+    }
+  }
+
+  return parameters.empty();
+}
 
 RCLCPP_LOCAL
 rcl_interfaces::msg::SetParametersResult
-__call_on_parameters_set_callbacks(
+__call_on_set_parameters_callbacks(
   const std::vector<rclcpp::Parameter> & parameters,
-  CallbacksContainerType & callback_container)
+  OnSetCallbacksHandleContainer & callback_container
+  )
 {
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
@@ -336,12 +375,32 @@ __call_on_parameters_set_callbacks(
   return result;
 }
 
+RCLCPP_LOCAL
+void __call_post_set_parameters_callbacks(  
+    const std::vector<rclcpp::Parameter> & parameters,
+    PostSetCallbacksHandleContainer & callback_container)
+{
+  if(!callback_container.empty()){
+    auto it = callback_container.begin();
+    while (it != callback_container.end()) {
+      auto shared_handle = it->lock();
+      if (nullptr != shared_handle) {
+        shared_handle->callback(parameters);
+        it++;
+      } else {
+        it = callback_container.erase(it);
+      }
+    }
+  }
+}
+
 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,
+  OnSetCallbacksHandleContainer & on_set_callback_container,
+  PostSetCallbacksHandleContainer & post_set_callback_container,
   bool allow_undeclared = false)
 {
   // Check if the value being set complies with the descriptor.
@@ -352,7 +411,8 @@ __set_parameters_atomically_common(
   }
   // Call the user callbacks to see if the new value(s) are allowed.
   result =
-    __call_on_parameters_set_callbacks(parameters, callback_container);
+    __call_on_set_parameters_callbacks(parameters, on_set_callback_container);
+  
   if (!result.successful) {
     return result;
   }
@@ -364,6 +424,9 @@ __set_parameters_atomically_common(
       parameter_infos[name].descriptor.type = parameters[i].get_type();
       parameter_infos[name].value = parameters[i].get_parameter_value();
     }
+    
+    // Call the user post set parameter callback 
+    __call_post_set_parameters_callbacks(parameters, post_set_callback_container);
   }
 
   // Either way, return the result.
@@ -378,7 +441,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,
-  CallbacksContainerType & callback_container,
+  OnSetCallbacksHandleContainer & on_set_callback_container,
+  PostSetCallbacksHandleContainer & post_set_callback_container,
   rcl_interfaces::msg::ParameterEvent * parameter_event_out,
   bool ignore_override = false)
 {
@@ -414,7 +478,9 @@ __declare_parameter_common(
   auto result = __set_parameters_atomically_common(
     parameter_wrappers,
     parameter_infos,
-    callback_container);
+    on_set_callback_container,
+    post_set_callback_container
+    );
 
   if (!result.successful) {
     return result;
@@ -441,11 +507,13 @@ declare_parameter_helper(
   bool ignore_override,
   std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & parameters,
   const std::map<std::string, rclcpp::ParameterValue> & overrides,
-  CallbacksContainerType & callback_container,
+  OnSetCallbacksHandleContainer & on_set_callback_container,
+  PostSetCallbacksHandleContainer & post_set_callback_container,
   rclcpp::Publisher<rcl_interfaces::msg::ParameterEvent> * events_publisher,
   const std::string & combined_name,
   rclcpp::node_interfaces::NodeClockInterface & node_clock)
 {
+
   // TODO(sloretz) parameter name validation
   if (name.empty()) {
     throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty");
@@ -477,7 +545,8 @@ declare_parameter_helper(
     parameter_descriptor,
     parameters,
     overrides,
-    callback_container,
+    on_set_callback_container,
+    post_set_callback_container,
     &parameter_event,
     ignore_override);
 
@@ -524,7 +593,8 @@ NodeParameters::declare_parameter(
     ignore_override,
     parameters_,
     parameter_overrides_,
-    on_parameters_set_callback_container_,
+    on_set_parameters_callback_container_,
+    post_set_parameter_callback_container_,
     events_publisher_.get(),
     combined_name_,
     *node_clock_);
@@ -559,7 +629,8 @@ NodeParameters::declare_parameter(
     ignore_override,
     parameters_,
     parameter_overrides_,
-    on_parameters_set_callback_container_,
+    on_set_parameters_callback_container_,
+    post_set_parameter_callback_container_,
     events_publisher_.get(),
     combined_name_,
     *node_clock_);
@@ -633,12 +704,25 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
 
   rcl_interfaces::msg::SetParametersResult result;
 
+  // call any user registered pre set parameter callbacks
+  // this callback can make changes to the original parameters list
+  // also check if the changed parameter list is empty or not, if empty return
+  std::vector<rclcpp::Parameter> parameters_after_pre_set_callback(parameters);
+  if(__call_pre_set_parameters_callbacks(parameters_after_pre_set_callback,
+                                         pre_set_parameter_callback_container_))
+  {
+    result.successful = false;
+    result.reason = "parameter list cannot be empty, this might be due to "
+                    "pre_set_parameters_callback modifying the original parameters list";
+    return result;
+  }
+
   // Check if any of the parameters are read-only, or if any parameters are not
   // declared.
   // If not declared, keep track of them in order to declare them later, when
   // undeclared parameters are allowed, and if they're not allowed, fail.
   std::vector<const rclcpp::Parameter *> parameters_to_be_declared;
-  for (const auto & parameter : parameters) {
+  for (const auto & parameter : parameters_after_pre_set_callback) {
     const std::string & name = parameter.get_name();
 
     // Check to make sure the parameter name is valid.
@@ -678,7 +762,8 @@ 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;
+  OnSetCallbacksHandleContainer empty_on_set_callback_container;
+  PostSetCallbacksHandleContainer empty_post_set_callback_container;
 
   // Implicit declare uses dynamic type descriptor.
   rcl_interfaces::msg::ParameterDescriptor descriptor;
@@ -693,7 +778,8 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
       staged_parameter_changes,
       parameter_overrides_,
       // Only call callbacks once below
-      empty_callback_container,  // callback_container is explicitly empty
+      empty_on_set_callback_container,   // callback_container is explicitly empty
+      empty_post_set_callback_container, // callback_container is explicitly empty
       &parameter_event_msg,
       true);
     if (!result.successful) {
@@ -706,12 +792,12 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
   // If there were implicitly declared parameters, then we may need to copy the input parameters
   // and then assign the value that was selected after the declare (could be affected by the
   // initial parameter values).
-  const std::vector<rclcpp::Parameter> * parameters_to_be_set = &parameters;
+  const std::vector<rclcpp::Parameter> * parameters_to_be_set = &parameters_after_pre_set_callback;
   std::vector<rclcpp::Parameter> parameters_copy;
   if (0 != staged_parameter_changes.size()) {  // If there were any implicitly declared parameters.
     bool any_initial_values_used = false;
     for (const auto & staged_parameter_change : staged_parameter_changes) {
-      auto it = __find_parameter_by_name(parameters, staged_parameter_change.first);
+      auto it = __find_parameter_by_name(parameters_after_pre_set_callback, staged_parameter_change.first);
       if (it->get_parameter_value() != staged_parameter_change.second.value) {
         // In this case, the value of the staged parameter differs from the
         // input from the user, and therefore we need to update things before setting.
@@ -721,7 +807,7 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
       }
     }
     if (any_initial_values_used) {
-      parameters_copy = parameters;
+      parameters_copy = parameters_after_pre_set_callback;
       for (const auto & staged_parameter_change : staged_parameter_changes) {
         auto it = __find_parameter_by_name(parameters_copy, staged_parameter_change.first);
         *it = Parameter(staged_parameter_change.first, staged_parameter_change.second.value);
@@ -754,8 +840,9 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
     // they are actually set on the official parameter storage
     parameters_,
     // These callbacks are called once. When a callback returns an unsuccessful result,
-    // the remaining aren't called.
-    on_parameters_set_callback_container_,
+    // the remaining aren't called
+    on_set_parameters_callback_container_,
+    post_set_parameter_callback_container_,
     allow_undeclared_);  // allow undeclared
 
   // If not successful, then stop here.
@@ -811,7 +898,7 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
     parameter_event_msg.stamp = node_clock_->get_clock()->now();
     events_publisher_->publish(parameter_event_msg);
   }
-
+  
   return result;
 }
 
@@ -997,6 +1084,26 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
   return result;
 }
 
+void
+NodeParameters::remove_pre_set_parameters_callback(
+    const PreSetParametersCallbackHandle * const handle)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  auto it = std::find_if(
+      pre_set_parameter_callback_container_.begin(),
+      pre_set_parameter_callback_container_.end(),
+      [handle](const auto & weak_handle) {
+        return handle == weak_handle.lock().get();
+      });
+  if (it != pre_set_parameter_callback_container_.end()) {
+    pre_set_parameter_callback_container_.erase(it);
+  } else {
+    throw std::runtime_error("Pre set parameter callback doesn't exist");
+  }
+}
+
 void
 NodeParameters::remove_on_set_parameters_callback(
   const OnSetParametersCallbackHandle * const handle)
@@ -1005,20 +1112,53 @@ NodeParameters::remove_on_set_parameters_callback(
   ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
 
   auto it = std::find_if(
-    on_parameters_set_callback_container_.begin(),
-    on_parameters_set_callback_container_.end(),
+    on_set_parameters_callback_container_.begin(),
+    on_set_parameters_callback_container_.end(),
     [handle](const auto & weak_handle) {
       return handle == weak_handle.lock().get();
     });
-  if (it != on_parameters_set_callback_container_.end()) {
-    on_parameters_set_callback_container_.erase(it);
+  if (it != on_set_parameters_callback_container_.end()) {
+    on_set_parameters_callback_container_.erase(it);
   } else {
-    throw std::runtime_error("Callback doesn't exist");
+    throw std::runtime_error("On set parameter callback doesn't exist");
   }
 }
 
+void
+NodeParameters::remove_post_set_parameters_callback(
+    const PostSetParametersCallbackHandle * const handle)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  auto it = std::find_if(
+      post_set_parameter_callback_container_.begin(),
+      post_set_parameter_callback_container_.end(),
+      [handle](const auto & weak_handle) {
+        return handle == weak_handle.lock().get();
+      });
+  if (it != post_set_parameter_callback_container_.end()) {
+    post_set_parameter_callback_container_.erase(it);
+  } else {
+    throw std::runtime_error("Post set parameter callback doesn't exist");
+  }
+}
+
+PreSetParametersCallbackHandle::SharedPtr
+NodeParameters::add_pre_set_parameters_callback(PreSetParametersCallbackType callback)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  auto handle = std::make_shared<PreSetParametersCallbackHandle>();
+  handle->callback = callback;
+  // the last callback registered is executed first.
+  pre_set_parameter_callback_container_.emplace_front(handle);
+  return handle;
+}
+
 OnSetParametersCallbackHandle::SharedPtr
-NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callback)
+NodeParameters::add_on_set_parameters_callback(OnSetParametersCallbackType callback)
 {
   std::lock_guard<std::recursive_mutex> lock(mutex_);
   ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
@@ -1026,7 +1166,19 @@ NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callb
   auto handle = std::make_shared<OnSetParametersCallbackHandle>();
   handle->callback = callback;
   // the last callback registered is executed first.
-  on_parameters_set_callback_container_.emplace_front(handle);
+  on_set_parameters_callback_container_.emplace_front(handle);
+  return handle;
+}
+
+PostSetParametersCallbackHandle::SharedPtr
+NodeParameters::add_post_set_parameters_callback(PostSetParametersCallbackType callback){
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
+  auto handle = std::make_shared<PostSetParametersCallbackHandle>();
+  handle->callback = callback;
+  // the last callback registered is executed first.
+  post_set_parameter_callback_container_.emplace_front(handle);
   return handle;
 }
 

rclcpp/src/rclcpp/node_options.cpp

@@ -75,6 +75,7 @@ NodeOptions::operator=(const NodeOptions & other)
     this->enable_rosout_ = other.enable_rosout_;
     this->use_intra_process_comms_ = other.use_intra_process_comms_;
     this->enable_topic_statistics_ = other.enable_topic_statistics_;
+    this->enable_service_introspection_ = other.enable_service_introspection_;
     this->start_parameter_services_ = other.start_parameter_services_;
     this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_;
     this->clock_qos_ = other.clock_qos_;
@@ -227,6 +228,18 @@ NodeOptions::enable_topic_statistics() const
   return this->enable_topic_statistics_;
 }
 
+bool
+NodeOptions::enable_service_introspection() const
+{
+  return this->enable_service_introspection_;
+}
+
+NodeOptions &
+NodeOptions::enable_service_introspection(bool enable_service_introspection) {
+  this->enable_service_introspection_ = enable_service_introspection;
+  return *this;
+}
+
 NodeOptions &
 NodeOptions::enable_topic_statistics(bool enable_topic_statistics)
 {

rclcpp/src/rclcpp/parameter_service.cpp

@@ -20,21 +20,25 @@
 #include <vector>
 
 #include "rclcpp/logging.hpp"
+#include "rclcpp/create_service.hpp"
 
 #include "./parameter_service_names.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 
 using rclcpp::ParameterService;
 
 ParameterService::ParameterService(
   const std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base,
   const std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services,
+  const std::shared_ptr<rclcpp::node_interfaces::NodeClockInterface> node_clock,
   rclcpp::node_interfaces::NodeParametersInterface * node_params,
+  bool enable_service_introspection,
   const rmw_qos_profile_t & qos_profile)
 {
   const std::string node_name = node_base->get_name();
 
   get_parameters_service_ = create_service<rcl_interfaces::srv::GetParameters>(
-    node_base, node_services,
+    node_base, node_services, node_clock,
     node_name + "/" + parameter_service_names::get_parameters,
     [node_params](
       const std::shared_ptr<rmw_request_id_t>,
@@ -50,10 +54,10 @@ ParameterService::ParameterService(
         RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what());
       }
     },
-    qos_profile, nullptr);
+    qos_profile, nullptr, enable_service_introspection);
 
   get_parameter_types_service_ = create_service<rcl_interfaces::srv::GetParameterTypes>(
-    node_base, node_services,
+    node_base, node_services, node_clock,
     node_name + "/" + parameter_service_names::get_parameter_types,
     [node_params](
       const std::shared_ptr<rmw_request_id_t>,
@@ -71,10 +75,10 @@ ParameterService::ParameterService(
         RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what());
       }
     },
-    qos_profile, nullptr);
+    qos_profile, nullptr, enable_service_introspection);
 
   set_parameters_service_ = create_service<rcl_interfaces::srv::SetParameters>(
-    node_base, node_services,
+    node_base, node_services, node_clock,
     node_name + "/" + parameter_service_names::set_parameters,
     [node_params](
       const std::shared_ptr<rmw_request_id_t>,
@@ -96,10 +100,10 @@ ParameterService::ParameterService(
         response->results.push_back(result);
       }
     },
-    qos_profile, nullptr);
+    qos_profile, nullptr, enable_service_introspection);
 
   set_parameters_atomically_service_ = create_service<rcl_interfaces::srv::SetParametersAtomically>(
-    node_base, node_services,
+    node_base, node_services, node_clock,
     node_name + "/" + parameter_service_names::set_parameters_atomically,
     [node_params](
       const std::shared_ptr<rmw_request_id_t>,
@@ -123,10 +127,10 @@ ParameterService::ParameterService(
         response->result.reason = "One or more parameters were not declared before setting";
       }
     },
-    qos_profile, nullptr);
+    qos_profile, nullptr, enable_service_introspection);
 
   describe_parameters_service_ = create_service<rcl_interfaces::srv::DescribeParameters>(
-    node_base, node_services,
+    node_base, node_services, node_clock,
     node_name + "/" + parameter_service_names::describe_parameters,
     [node_params](
       const std::shared_ptr<rmw_request_id_t>,
@@ -140,10 +144,10 @@ ParameterService::ParameterService(
         RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what());
       }
     },
-    qos_profile, nullptr);
+    qos_profile, nullptr, enable_service_introspection);
 
   list_parameters_service_ = create_service<rcl_interfaces::srv::ListParameters>(
-    node_base, node_services,
+    node_base, node_services, node_clock,
     node_name + "/" + parameter_service_names::list_parameters,
     [node_params](
       const std::shared_ptr<rmw_request_id_t>,
@@ -153,5 +157,5 @@ ParameterService::ParameterService(
       auto result = node_params->list_parameters(request->prefixes, request->depth);
       response->result = result;
     },
-    qos_profile, nullptr);
+    qos_profile, nullptr, enable_service_introspection);
 }

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -170,7 +170,7 @@ TEST_F(TestNodeParameters, set_parameters) {
   EXPECT_TRUE(result[0].successful);
 }
 
-TEST_F(TestNodeParameters, add_remove_parameters_callback) {
+TEST_F(TestNodeParameters, add_remove_on_set_parameters_callback) {
   rcl_interfaces::msg::ParameterDescriptor bool_descriptor;
   bool_descriptor.name = "bool_parameter";
   bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
@@ -197,5 +197,124 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) {
 
   RCLCPP_EXPECT_THROW_EQ(
     node_parameters->remove_on_set_parameters_callback(handle.get()),
-    std::runtime_error("Callback doesn't exist"));
+    std::runtime_error("On set parameter callback doesn't exist"));
+}
+
+TEST_F(TestNodeParameters, add_remove_pre_set_parameters_callback) {
+  rcl_interfaces::msg::ParameterDescriptor param1_descriptor;
+  param1_descriptor.name = "param1";
+  param1_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  param1_descriptor.read_only = false;
+
+  rcl_interfaces::msg::ParameterDescriptor param2_descriptor;
+  param1_descriptor.name = "param2";
+  param1_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  param1_descriptor.read_only = false;
+
+  node_parameters->declare_parameter(
+      "param1", rclcpp::ParameterValue(0.0), param1_descriptor, false);
+  node_parameters->declare_parameter(
+      "param2", rclcpp::ParameterValue(0.0), param2_descriptor, false);
+
+  const std::vector<rclcpp::Parameter> parameters_to_be_set = {rclcpp::Parameter("param1", 1.0)};
+
+  //  use "pre set callback" to change param2 conditioned on changes to param1.
+  {
+    auto callback = [](std::vector<rclcpp::Parameter> &parameters) {
+      for (const auto &param: parameters) {
+        if (param.get_name() == "param1") {
+          parameters.emplace_back("param2", 2.0);
+        }
+      }
+    };
+
+    auto handle = node_parameters->add_pre_set_parameters_callback(callback);
+    auto result = node_parameters->set_parameters(parameters_to_be_set);
+    // we expect the result size to be same as the original "parameters_to_be_set"
+    // since the pre set parameter callback will set the modified param list atomically.
+    ASSERT_EQ(1u, result.size());
+    EXPECT_TRUE(result[0].successful);
+    EXPECT_TRUE(node_parameters->has_parameter("param1"));
+    EXPECT_EQ(node_parameters->get_parameter("param1").get_value<double>(), 1.0);
+    EXPECT_TRUE(node_parameters->has_parameter("param2"));
+    EXPECT_EQ(node_parameters->get_parameter("param2").get_value<double>(), 2.0);
+
+    EXPECT_NO_THROW(node_parameters->remove_pre_set_parameters_callback(handle.get()));
+
+    RCLCPP_EXPECT_THROW_EQ(
+        node_parameters->remove_pre_set_parameters_callback(handle.get()),
+        std::runtime_error("Pre set parameter callback doesn't exist"));
+  }
+
+  // the result will be unsuccessful if the pre set callback makes parameter list empty
+  {
+    auto callback = [](std::vector<rclcpp::Parameter> &parameters) {
+      parameters = {};
+    };
+
+    std::string reason = "parameter list cannot be empty, this might be due to "
+                  "pre_set_parameters_callback modifying the original parameters list";
+    auto handle = node_parameters->add_pre_set_parameters_callback(callback);
+    auto results = node_parameters->set_parameters(parameters_to_be_set);
+    EXPECT_FALSE(results[0].successful);
+    EXPECT_EQ(results[0].reason, reason);
+    EXPECT_NO_THROW(node_parameters->remove_pre_set_parameters_callback(handle.get()));
+
+    RCLCPP_EXPECT_THROW_EQ(
+        node_parameters->remove_pre_set_parameters_callback(handle.get()),
+        std::runtime_error("Pre set parameter callback doesn't exist"));
+  }
+}
+
+TEST_F(TestNodeParameters, add_remove_post_set_parameters_callback) {
+  rcl_interfaces::msg::ParameterDescriptor param1_descriptor;
+  param1_descriptor.name = "double_parameter1";
+  param1_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  param1_descriptor.read_only = false;
+
+  rcl_interfaces::msg::ParameterDescriptor param2_descriptor;
+  param2_descriptor.name = "double_parameter2";
+  param2_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
+  param2_descriptor.read_only = false;
+
+  node_parameters->declare_parameter(
+      "param1", rclcpp::ParameterValue(0.0),param1_descriptor, false);
+  node_parameters->declare_parameter(
+      "param2", rclcpp::ParameterValue(0.0),param2_descriptor, false);
+
+  double variable_tracking_param1_internally = node_parameters->get_parameter("param1").get_value<double>();
+  double variable_tracking_param2_internally = node_parameters->get_parameter("param2").get_value<double>();
+
+  EXPECT_EQ(variable_tracking_param1_internally, 0.0);
+  EXPECT_EQ(variable_tracking_param2_internally, 0.0);
+
+  const std::vector<rclcpp::Parameter> parameters_to_be_set = {rclcpp::Parameter("param1", 1.0),
+                                                               rclcpp::Parameter("param2", 2.0)};
+
+  // register a callback for successful set parameter and change the internally tracked variables.
+  auto callback = [&](const std::vector<rclcpp::Parameter> &parameters) {
+    for(const auto& param: parameters){
+      if(param.get_name() == "param1"){
+        variable_tracking_param1_internally = param.get_value<double>();
+      }else if(param.get_name() == "param2"){
+        variable_tracking_param2_internally = param.get_value<double>();
+      }
+    }
+  };
+
+  auto handle = node_parameters->add_post_set_parameters_callback(callback);
+  auto result = node_parameters->set_parameters(parameters_to_be_set);
+  ASSERT_EQ(2u, result.size());
+  EXPECT_TRUE(result[0].successful);
+  EXPECT_TRUE(result[1].successful);
+  EXPECT_TRUE(node_parameters->has_parameter("param1"));
+  EXPECT_TRUE(node_parameters->has_parameter("param2"));
+  EXPECT_EQ(variable_tracking_param1_internally, 1.0);
+  EXPECT_EQ(variable_tracking_param2_internally, 2.0);
+
+  EXPECT_NO_THROW(node_parameters->remove_post_set_parameters_callback(handle.get()));
+
+  RCLCPP_EXPECT_THROW_EQ(
+      node_parameters->remove_post_set_parameters_callback(handle.get()),
+      std::runtime_error("Post set parameter callback doesn't exist"));
 }

rclcpp/test/rclcpp/test_client.cpp

@@ -107,9 +107,11 @@ TEST_F(TestClient, construction_with_free_function) {
       node->get_node_base_interface(),
       node->get_node_graph_interface(),
       node->get_node_services_interface(),
+      node->get_node_clock_interface(),
       "service",
       rmw_qos_profile_services_default,
-      nullptr);
+      nullptr,
+      true);
   }
   {
     ASSERT_THROW(
@@ -118,9 +120,11 @@ TEST_F(TestClient, construction_with_free_function) {
         node->get_node_base_interface(),
         node->get_node_graph_interface(),
         node->get_node_services_interface(),
+        node->get_node_clock_interface(),
         "invalid_?service",
         rmw_qos_profile_services_default,
-        nullptr);
+        nullptr,
+        true);
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }

rclcpp/test/rclcpp/test_node.cpp

@@ -1444,6 +1444,38 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) {
     EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING);
     EXPECT_EQ(value.get_value<std::string>(), "asd");
   }
+  {
+    // adding a parameter in "pre set parameter" callback, when that
+    // parameter has not been declared before will throw
+    auto name1 = "parameter"_unq;
+    auto name2 = "parameter"_unq;
+    auto default_value = 0;  // default value of name1 param
+
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.type = rclcpp::PARAMETER_INTEGER;
+
+    // declare name1 parameter only
+    node->declare_parameter(name1, default_value, descriptor);
+
+    // add undeclared parameter with name2 to modified list of parameters
+    auto pre_set_parameters =
+        [&](std::vector<rclcpp::Parameter> &parameters) {
+        for(const auto&param: parameters){
+          if(param.get_name() == name1){
+            parameters.emplace_back(rclcpp::Parameter(name2, 2));
+          }
+        }
+    };
+
+    auto handler = node->add_pre_set_parameters_callback(pre_set_parameters);
+    EXPECT_THROW(node->set_parameter(rclcpp::Parameter(name1, 4)),
+                 rclcpp::exceptions::ParameterNotDeclaredException);
+    EXPECT_TRUE(node->has_parameter(name1));
+    EXPECT_EQ(node->get_parameter(name1).get_value<int>(), default_value);
+    EXPECT_FALSE(node->has_parameter(name2));
+    RCPPUTILS_SCOPE_EXIT(
+        {node->remove_pre_set_parameters_callback(handler.get());});  // always reset
+  }
 }
 
 TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) {
@@ -1481,6 +1513,39 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) {
     EXPECT_TRUE(node->has_parameter(name));
     EXPECT_EQ(node->get_parameter(name).get_value<int>(), 43);
   }
+  {
+    // adding a parameter in "pre set parameter" callback, when that
+    // parameter has not been declared will not throw if undeclared
+    // parameters are allowed
+    auto name1 = "parameter"_unq;
+    auto name2 = "parameter"_unq;
+
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.type = rclcpp::PARAMETER_INTEGER;
+
+    // declare name1 parameter only
+    node->declare_parameter(name1, 0, descriptor);
+
+    // add undeclared parameter with name2 to modified list of parameters
+    auto pre_set_parameters =
+        [&](std::vector<rclcpp::Parameter> &parameters) {
+          for(const auto&param: parameters){
+            if(param.get_name() == name1){
+              parameters.emplace_back(rclcpp::Parameter(name2, 2));
+            }
+          }
+        };
+
+    auto handler = node->add_pre_set_parameters_callback(pre_set_parameters);
+    auto result = node->set_parameter(rclcpp::Parameter(name1, 1));
+    EXPECT_TRUE(result.successful);
+    EXPECT_TRUE(node->has_parameter(name1));
+    EXPECT_TRUE(node->has_parameter(name2));
+    EXPECT_EQ(node->get_parameter(name1).get_value<int>(), 1);
+    EXPECT_EQ(node->get_parameter(name2).get_value<int>(), 2);
+    RCPPUTILS_SCOPE_EXIT(
+        {node->remove_pre_set_parameters_callback(handler.get());});  // always reset
+  }
 }
 
 TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) {
@@ -1625,6 +1690,50 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) {
 
     EXPECT_FALSE(node->has_parameter(name));
   }
+  {
+    // adding a parameter in "pre set parameter" callback when that
+    // parameter has not been declared before will throw. However, when
+    // multiple params are being set using "set_parameters", the params
+    // which are not conditioned on each other in "pre set callback" will
+    // still be set successfully. This is the desired behaviour since
+    // "set_parameters" sets params non atomically.
+    auto name1 = "parameter"_unq;
+    auto name2 = "parameter"_unq;
+    auto name3 = "parameter"_unq;
+    auto default_value = 0;
+
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.type = rclcpp::PARAMETER_INTEGER;
+
+    // declare name1 and name2 parameter only
+    node->declare_parameter(name1, default_value, descriptor);
+    node->declare_parameter(name2, default_value, descriptor);
+
+    // add undeclared parameter with name3 to modified list of parameters
+    // conditioned of name2 param
+    auto pre_set_parameters =
+        [&](std::vector<rclcpp::Parameter> &parameters) {
+          for(const auto&param: parameters){
+            if(param.get_name() == name2){
+              parameters.emplace_back(rclcpp::Parameter(name3, 3));
+            }
+          }
+        };
+
+    auto handler = node->add_pre_set_parameters_callback(pre_set_parameters);
+    EXPECT_THROW(node->set_parameters({rclcpp::Parameter(name1, 1), rclcpp::Parameter(name2, 2)}),
+                 rclcpp::exceptions::ParameterNotDeclaredException);
+    EXPECT_TRUE(node->has_parameter(name1));
+    EXPECT_TRUE(node->has_parameter(name2));
+    EXPECT_FALSE(node->has_parameter(name3));
+
+    // we still expect the value of name1 param to be set successfully, since
+    // the setting of name2 param is only conditioned on setting of name3 param
+    EXPECT_EQ(node->get_parameter(name1).get_value<int>(), 1);
+    EXPECT_EQ(node->get_parameter(name2).get_value<int>(), default_value);
+    RCPPUTILS_SCOPE_EXIT(
+        {node->remove_pre_set_parameters_callback(handler.get());});  // always reset
+  }
 }
 
 // test set_parameters with undeclared allowed
@@ -1673,6 +1782,48 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) {
     EXPECT_EQ(node->get_parameter(name1).get_value<int>(), 42);
     EXPECT_EQ(node->get_parameter(name2).get_value<std::string>(), "test");
   }
+  {
+    // adding a parameter in "pre set parameter" callback when that
+    // parameter has not been declared before will not throw when
+    // undeclared parameters are allowed.
+    auto name1 = "parameter"_unq;
+    auto name2 = "parameter"_unq;
+    auto name3 = "parameter"_unq;
+    auto default_value = 0;
+
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.type = rclcpp::PARAMETER_INTEGER;
+
+    // declare name1 and name2 parameter only
+    node->declare_parameter(name1, default_value, descriptor);
+    node->declare_parameter(name2, default_value, descriptor);
+
+    // add undeclared parameter with name3 to modified list of parameters
+    // conditioned of name2 param
+    auto pre_set_parameters =
+        [&](std::vector<rclcpp::Parameter> &parameters) {
+          for(const auto&param: parameters){
+            if(param.get_name() == name2){
+              parameters.emplace_back(rclcpp::Parameter(name3, 3));
+            }
+          }
+        };
+
+    auto handler = node->add_pre_set_parameters_callback(pre_set_parameters);
+    auto results = node->set_parameters({rclcpp::Parameter(name1, 1), rclcpp::Parameter(name2, 2)});
+    EXPECT_EQ(2u, results.size());
+    EXPECT_TRUE(results[0].successful);
+    EXPECT_TRUE(results[1].successful);
+    EXPECT_TRUE(node->has_parameter(name1));
+    EXPECT_TRUE(node->has_parameter(name2));
+    EXPECT_TRUE(node->has_parameter(name3));
+    EXPECT_EQ(node->get_parameter(name1).get_value<int>(), 1);
+    EXPECT_EQ(node->get_parameter(name2).get_value<int>(), 2);
+    EXPECT_EQ(node->get_parameter(name3).get_value<int>(), 3);
+
+    RCPPUTILS_SCOPE_EXIT(
+        {node->remove_pre_set_parameters_callback(handler.get());});  // always reset
+  }
 }
 
 TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) {
@@ -1815,6 +1966,49 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) {
 
     EXPECT_FALSE(node->has_parameter(name));
   }
+  {
+    // adding a parameter in "pre set parameter" callback when that
+    // parameter has not been declared before will throw and since
+    // multiple params are being set using "set_parameters_atomically",
+    // a failure in set of one param will result in all params being
+    // set unsuccessfully.
+    auto name1 = "parameter"_unq;
+    auto name2 = "parameter"_unq;
+    auto name3 = "parameter"_unq;
+    auto default_value = 0;
+
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.type = rclcpp::PARAMETER_INTEGER;
+
+    // declare name1 and name2 parameter only
+    node->declare_parameter(name1, default_value, descriptor);
+    node->declare_parameter(name2, default_value, descriptor);
+
+    // add undeclared parameter with name3 to modified list of parameters
+    // conditioned of name2 param
+    auto pre_set_parameters =
+        [&](std::vector<rclcpp::Parameter> &parameters) {
+          for(const auto&param: parameters){
+            if(param.get_name() == name2){
+              parameters.emplace_back(rclcpp::Parameter(name3, 3));
+            }
+          }
+        };
+
+    auto handler = node->add_pre_set_parameters_callback(pre_set_parameters);
+    EXPECT_THROW(node->set_parameters_atomically({rclcpp::Parameter(name1, 1),
+                                                            rclcpp::Parameter(name2, 2)}),
+                 rclcpp::exceptions::ParameterNotDeclaredException);
+    EXPECT_TRUE(node->has_parameter(name1));
+    EXPECT_TRUE(node->has_parameter(name2));
+    EXPECT_FALSE(node->has_parameter(name3));
+
+    // the values of all the params is still default.
+    EXPECT_EQ(node->get_parameter(name1).get_value<int>(), default_value);
+    EXPECT_EQ(node->get_parameter(name2).get_value<int>(), default_value);
+    RCPPUTILS_SCOPE_EXIT(
+        {node->remove_pre_set_parameters_callback(handler.get());});  // always reset
+  }
 }
 
 // test set_parameters with undeclared allowed
@@ -1903,6 +2097,47 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) {
     EXPECT_FALSE(node->has_parameter(name2));  // important! name2 remains undeclared
     EXPECT_EQ(node->get_parameter(name3).get_value<std::string>(), "test");
   }
+  {
+    // adding a parameter in "pre set parameter" callback when that
+    // parameter has not been declared before will not throw when
+    // undeclared parameters are allowed.
+    auto name1 = "parameter"_unq;
+    auto name2 = "parameter"_unq;
+    auto name3 = "parameter"_unq;
+    auto default_value = 0;
+
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.type = rclcpp::PARAMETER_INTEGER;
+
+    // declare name1 and name2 parameter only
+    node->declare_parameter(name1, default_value, descriptor);
+    node->declare_parameter(name2, default_value, descriptor);
+
+    // add undeclared parameter with name3 to modified list of parameters
+    // conditioned of name2 param
+    auto pre_set_parameters =
+        [&](std::vector<rclcpp::Parameter> &parameters) {
+          for(const auto&param: parameters){
+            if(param.get_name() == name2){
+              parameters.emplace_back(rclcpp::Parameter(name3, 3));
+            }
+          }
+        };
+
+    auto handler = node->add_pre_set_parameters_callback(pre_set_parameters);
+    auto result = node->set_parameters_atomically({rclcpp::Parameter(name1, 1),
+                                                   rclcpp::Parameter(name2, 2)});
+    EXPECT_TRUE(result.successful);
+    EXPECT_TRUE(node->has_parameter(name1));
+    EXPECT_TRUE(node->has_parameter(name2));
+    EXPECT_TRUE(node->has_parameter(name3));
+    EXPECT_EQ(node->get_parameter(name1).get_value<int>(), 1);
+    EXPECT_EQ(node->get_parameter(name2).get_value<int>(), 2);
+    EXPECT_EQ(node->get_parameter(name3).get_value<int>(), 3);
+
+    RCPPUTILS_SCOPE_EXIT(
+        {node->remove_pre_set_parameters_callback(handler.get());});  // always reset
+  }
 }
 
 // test get_parameter with undeclared not allowed