enable parameter update recursively only when QoS override parameters.

rclcpp/include/rclcpp/detail/qos_parameters.hpp

@@ -97,6 +97,9 @@ declare_parameter_or_get(
   rcl_interfaces::msg::ParameterDescriptor descriptor)
 {
   try {
+    // enable parameter modification to make it possible
+    // to declare QoS override parameters during parameter callbacks.
+    parameters_interface.enable_parameter_modification();
     return parameters_interface.declare_parameter(
       param_name, param_value, descriptor);
   } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) {

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -214,6 +214,10 @@ class NodeParameters : public NodeParametersInterface
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const override;
 
+  RCLCPP_PUBLIC
+  void
+  enable_parameter_modification() override;
+
   using PreSetCallbacksHandleContainer = std::list<PreSetParametersCallbackHandle::WeakPtr>;
   using OnSetCallbacksHandleContainer = std::list<OnSetParametersCallbackHandle::WeakPtr>;
   using PostSetCallbacksHandleContainer = std::list<PostSetParametersCallbackHandle::WeakPtr>;

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -270,6 +270,26 @@ class NodeParametersInterface
   virtual
   const std::map<std::string, rclcpp::ParameterValue> &
   get_parameter_overrides() const = 0;
+
+  /// Enable parameter modification recursively during parameter callbacks.
+  /**
+   * This function is used to enable parameter modification during parameter callbacks.
+   *
+   * There are times when it does not allow parameter modification, such as when the parameter
+   * callbacks are being called and tries to modify the parameters with set_parameter and
+   * declare_parameter to avoid recursive parameter modification.
+   * This is protected by rclcpp::node_interfaces::ParameterMutationRecursionGuard.
+   *
+   * This function is explicitly called to allow the recursive parameter operation during
+   * parameter callbacks by the application.
+   * Once it is enabled, the next parameter operation set/declare/undeclare_parameter are
+   * allowed to execute in the parameter callback. But, no more further recursive operation
+   * is allowed, unless user application calls this API again.
+   */
+  RCLCPP_PUBLIC
+  virtual
+  void
+  enable_parameter_modification() = 0;
 };
 
 }  // namespace node_interfaces

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -1191,3 +1191,10 @@ NodeParameters::get_parameter_overrides() const
 {
   return parameter_overrides_;
 }
+
+void
+NodeParameters::enable_parameter_modification()
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  parameter_modification_enabled_ = true;
+}

rclcpp/test/rclcpp/CMakeLists.txt

@@ -196,7 +196,7 @@ ament_add_gtest(test_node_interfaces__node_parameters
   node_interfaces/test_node_parameters.cpp)
 ament_add_test_label(test_node_interfaces__node_parameters mimick)
 if(TARGET test_node_interfaces__node_parameters)
-  target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick rcpputils::rcpputils)
+  target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick rcpputils::rcpputils ${test_msgs_TARGETS})
 endif()
 ament_add_gtest(test_node_interfaces__node_services
   node_interfaces/test_node_services.cpp)

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -29,6 +29,8 @@
 #include "rclcpp/node.hpp"
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 
+#include "test_msgs/msg/empty.hpp"
+
 #include "../../mocking_utils/patch.hpp"
 #include "../../utils/rclcpp_gtest_macros.hpp"
 
@@ -329,6 +331,89 @@ TEST_F(TestNodeParameters, add_remove_post_set_parameters_callback) {
     std::runtime_error("Post set parameter callback doesn't exist"));
 }
 
+TEST_F(TestNodeParameters, set_param_recursive_in_post_set_parameters_callback) {
+  rclcpp::Subscription<test_msgs::msg::Empty>::SharedPtr subscription_;
+  rclcpp::Publisher<test_msgs::msg::Empty>::SharedPtr publisher_;
+
+  rcl_interfaces::msg::ParameterDescriptor param_descriptor;
+  param_descriptor.name = "create_entities";
+  param_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  param_descriptor.read_only = false;
+
+  bool result = node_parameters->declare_parameter(
+    "create_entities", rclcpp::ParameterValue(false), param_descriptor, false).get<bool>();
+  EXPECT_EQ(result, false);
+
+  // Register a callback to create/delete publisher and subscription with
+  // QoS override parameter options. This will call declare_parameter recursively
+  // during this callback.
+  auto sub_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {};
+  auto callback = [&](const std::vector<rclcpp::Parameter> & parameters) {
+      for (const auto & parameter : parameters) {
+        if (parameter.get_name() == "create_entities" &&
+          parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL)
+        {
+          if (parameter.as_bool()) {
+            ASSERT_EQ(subscription_, nullptr);
+            rclcpp::SubscriptionOptions sub_options;
+            // This will declare the QoS override parameters in this callback.
+            sub_options.qos_overriding_options =
+              rclcpp::QosOverridingOptions::with_default_policies();
+            subscription_ = node->create_subscription<test_msgs::msg::Empty>(
+              "empty",
+              rclcpp::QoS(10),
+              sub_callback,
+              sub_options);
+            ASSERT_NE(subscription_, nullptr);
+            ASSERT_EQ(publisher_, nullptr);
+            rclcpp::PublisherOptions pub_options;
+            // This will declare the QoS override parameters in this callback.
+            pub_options.qos_overriding_options =
+              rclcpp::QosOverridingOptions::with_default_policies();
+            publisher_ = node->create_publisher<test_msgs::msg::Empty>(
+              "empty",
+              rclcpp::QoS(10),
+              pub_options);
+            ASSERT_NE(publisher_, nullptr);
+          } else {
+            ASSERT_NE(subscription_, nullptr);
+            subscription_.reset();
+            ASSERT_EQ(subscription_, nullptr);
+            ASSERT_NE(publisher_, nullptr);
+            publisher_.reset();
+            ASSERT_EQ(publisher_, nullptr);
+          }
+        }
+      }
+    };
+
+  auto handle = node_parameters->add_post_set_parameters_callback(callback);
+  ASSERT_NE(handle, nullptr);
+  EXPECT_TRUE(node_parameters->has_parameter("create_entities"));
+  EXPECT_EQ(node_parameters->get_parameter("create_entities").get_value<bool>(), false);
+
+  // This will call the registered callback, that will create endpoints with
+  // declaring the QoS override parameters recursively.
+  auto results = node_parameters->set_parameters({rclcpp::Parameter("create_entities", true)});
+  EXPECT_TRUE(!results.empty() && results[0].successful);
+
+  EXPECT_TRUE(node_parameters->has_parameter("create_entities"));
+  EXPECT_EQ(node_parameters->get_parameter("create_entities").get_value<bool>(), true);
+
+  // Destroy publisher and subscription endpoints.
+  results = node_parameters->set_parameters({rclcpp::Parameter("create_entities", false)});
+  EXPECT_TRUE(!results.empty() && results[0].successful);
+
+  EXPECT_TRUE(node_parameters->has_parameter("create_entities"));
+  EXPECT_EQ(node_parameters->get_parameter("create_entities").get_value<bool>(), false);
+
+  // Make sure recreation can also work without any exception.
+  results = node_parameters->set_parameters({rclcpp::Parameter("create_entities", true)});
+  EXPECT_TRUE(!results.empty() && results[0].successful);
+
+  EXPECT_NO_THROW(node_parameters->remove_post_set_parameters_callback(handle.get()));
+}
+
 TEST_F(TestNodeParameters, wildcard_with_namespace)
 {
   rclcpp::NodeOptions opts;