From 24e570b44d9cc4eeff7a47d674cd83d94e25e7a7 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 11 Feb 2025 20:38:06 -0800 Subject: [PATCH 1/4] enable parameter update recursively only when QoS override parameters. Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 3 +++ rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp | 4 ++++ .../rclcpp/node_interfaces/node_parameters_interface.hpp | 6 ++++++ rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 6 ++++++ 4 files changed, 19 insertions(+) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index 651e58e7d2..56c4f3d50a 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -97,6 +97,9 @@ declare_parameter_or_get( rcl_interfaces::msg::ParameterDescriptor descriptor) { try { + // enable parameter modification just once to make it possible + // to declare QoS override parameters. + parameters_interface.enable_parameter_modification(); return parameters_interface.declare_parameter( param_name, param_value, descriptor); } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index ffbb400c11..3d867090dd 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -214,6 +214,10 @@ class NodeParameters : public NodeParametersInterface const std::map & get_parameter_overrides() const override; + RCLCPP_PUBLIC + void + enable_parameter_modification() override; + using PreSetCallbacksHandleContainer = std::list; using OnSetCallbacksHandleContainer = std::list; using PostSetCallbacksHandleContainer = std::list; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index 1cf10c1a97..eba921eaa9 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -270,6 +270,12 @@ class NodeParametersInterface virtual const std::map & get_parameter_overrides() const = 0; + + /// Enable parameter modification recursively just once. + RCLCPP_PUBLIC + virtual + void + enable_parameter_modification() = 0; }; } // namespace node_interfaces diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 922ce9e4d1..1123927f9e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -1191,3 +1191,9 @@ NodeParameters::get_parameter_overrides() const { return parameter_overrides_; } + +void +NodeParameters::enable_parameter_modification() +{ + parameter_modification_enabled_ = true; +} From 7be24a7dd825ae2b8b06dfbd9c7d3efb62984dd4 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 7 Apr 2025 15:47:41 -0700 Subject: [PATCH 2/4] add docstring for NodeParametersInterface::enable_parameter_modification(). Signed-off-by: Tomoya Fujita --- .../node_interfaces/node_parameters_interface.hpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index eba921eaa9..82259dd363 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -271,7 +271,17 @@ class NodeParametersInterface const std::map & get_parameter_overrides() const = 0; - /// Enable parameter modification recursively just once. + /// Enable parameter modification recursively during parameter callbacks. + /** + * This function is used to enable parameter modification during parameter operations. + * + * 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 function is explicitly called to allow the recursive parameter operation during + * parameter callbacks by the application. + */ RCLCPP_PUBLIC virtual void From d4f34a1a700540a5a9a1253897987092f732c03c Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 7 Apr 2025 23:49:36 -0700 Subject: [PATCH 3/4] add set_param_recursive_in_post_set_parameters_callback test. Signed-off-by: Tomoya Fujita --- rclcpp/test/rclcpp/CMakeLists.txt | 2 +- .../node_interfaces/test_node_parameters.cpp | 85 +++++++++++++++++++ 2 files changed, 86 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index b7f744f049..3ae3fa7306 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/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) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 0cc06792a8..5e851a8434 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/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::SharedPtr subscription_; + rclcpp::Publisher::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(); + 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 & 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( + "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( + "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(), 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(), 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(), 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; From 850405b22998d3e95e234b510eb3044a430a2297 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 8 Apr 2025 11:11:07 -0700 Subject: [PATCH 4/4] add more docstring and missing recursive mutex lock. Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 4 ++-- .../rclcpp/node_interfaces/node_parameters_interface.hpp | 6 +++++- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 1 + 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index 56c4f3d50a..1a9138e9c8 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -97,8 +97,8 @@ declare_parameter_or_get( rcl_interfaces::msg::ParameterDescriptor descriptor) { try { - // enable parameter modification just once to make it possible - // to declare QoS override parameters. + // 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); diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index 82259dd363..8636fdec74 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -273,14 +273,18 @@ class NodeParametersInterface /// Enable parameter modification recursively during parameter callbacks. /** - * This function is used to enable parameter modification during parameter operations. + * 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 diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 1123927f9e..33f9391917 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -1195,5 +1195,6 @@ NodeParameters::get_parameter_overrides() const void NodeParameters::enable_parameter_modification() { + std::lock_guard lock(mutex_); parameter_modification_enabled_ = true; }