Switch the NodeParameters lock to recursive.

rclcpp/include/rclcpp/exceptions.hpp

@@ -219,6 +219,13 @@ class ParameterImmutableException : public std::runtime_error
   using std::runtime_error::runtime_error;
 };
 
+/// Thrown if parameter is modified while in a set callback.
+class ParameterModifiedInCallbackException : public std::runtime_error
+{
+  // Inherit constructors from runtime_error.
+  using std::runtime_error::runtime_error;
+};
+
 }  // namespace exceptions
 }  // namespace rclcpp
 

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -151,6 +151,11 @@ class NodeParameters : public NodeParametersInterface
 
   mutable std::recursive_mutex mutex_;
 
+  // There are times when we don't want to allow modifications to parameters
+  // (particularly when a set_parameter callback tries to call set_parameter,
+  // declare_parameter, etc).  In those cases, this will be set to false.
+  bool parameter_modification_enabled_{true};
+
   OnParametersSetCallbackType on_parameters_set_callback_ = nullptr;
 
   std::map<std::string, ParameterInfo> parameters_;

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -376,6 +376,11 @@ NodeParameters::declare_parameter(
 {
   std::lock_guard<std::recursive_mutex> lock(mutex_);
 
+  if (!parameter_modification_enabled_) {
+    throw rclcpp::exceptions::ParameterModifiedInCallbackException(
+            "cannot declare a parameter from within set callback");
+  }
+
   // TODO(sloretz) parameter name validation
   if (name.empty()) {
     throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty");
@@ -388,6 +393,8 @@ NodeParameters::declare_parameter(
   }
 
   rcl_interfaces::msg::ParameterEvent parameter_event;
+  parameter_modification_enabled_ = false;
+  RCLCPP_SCOPE_EXIT({parameter_modification_enabled_ = true;});
   auto result = __declare_parameter_common(
     name,
     default_value,
@@ -397,6 +404,7 @@ NodeParameters::declare_parameter(
     on_parameters_set_callback_,
     &parameter_event,
     ignore_override);
+  parameter_modification_enabled_ = true;
 
   // If it failed to be set, then throw an exception.
   if (!result.successful) {
@@ -417,6 +425,11 @@ NodeParameters::undeclare_parameter(const std::string & name)
 {
   std::lock_guard<std::recursive_mutex> lock(mutex_);
 
+  if (!parameter_modification_enabled_) {
+    throw rclcpp::exceptions::ParameterModifiedInCallbackException(
+            "cannot declare a parameter from within set callback");
+  }
+
   auto parameter_info = parameters_.find(name);
   if (parameter_info == parameters_.end()) {
     throw rclcpp::exceptions::ParameterNotDeclaredException(
@@ -470,6 +483,11 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
 {
   std::lock_guard<std::recursive_mutex> lock(mutex_);
 
+  if (!parameter_modification_enabled_) {
+    throw rclcpp::exceptions::ParameterModifiedInCallbackException(
+            "cannot modify a parameter from within set callback");
+  }
+
   rcl_interfaces::msg::SetParametersResult result;
 
   // Check if any of the parameters are read-only, or if any parameters are not
@@ -518,6 +536,11 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
   rcl_interfaces::msg::ParameterEvent parameter_event_msg;
   parameter_event_msg.node = combined_name_;
   for (auto parameter_to_be_declared : parameters_to_be_declared) {
+    // Since the callback is explicitly nullptr in __declare_parameter_common,
+    // this isn't strictly needed.  We do it anyway because it is cheap and in
+    // case the implementation changes in the future.
+    parameter_modification_enabled_ = false;
+
     // This should not throw, because we validated the name and checked that
     // the parameter was not already declared.
     result = __declare_parameter_common(
@@ -529,6 +552,8 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
       nullptr,  // callback is explicitly null, so that it is called only once, when setting below.
       &parameter_event_msg,
       true);
+    parameter_modification_enabled_ = true;
+
     if (!result.successful) {
       // Declare failed, return knowing that nothing was changed because the
       // staged changes were not applied.
@@ -575,14 +600,17 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
     }
   }
 
-  // Set the all of the parameters including the ones declared implicitly above.
+  // Set all of the parameters including the ones declared implicitly above.
+  parameter_modification_enabled_ = false;
+  RCLCPP_SCOPE_EXIT({parameter_modification_enabled_ = true;});
   result = __set_parameters_atomically_common(
     // either the original parameters given by the user, or ones updated with initial values
     *parameters_to_be_set,
     // they are actually set on the official parameter storage
     parameters_,
     // this will get called once, with all the parameters to be set
     on_parameters_set_callback_);
+  parameter_modification_enabled_ = true;
 
   // If not successful, then stop here.
   if (!result.successful) {
@@ -817,6 +845,13 @@ NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint6
 NodeParameters::OnParametersSetCallbackType
 NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback)
 {
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  if (!parameter_modification_enabled_) {
+    throw rclcpp::exceptions::ParameterModifiedInCallbackException(
+            "cannot modify a parameter from within set callback");
+  }
+
   auto existing_callback = on_parameters_set_callback_;
   on_parameters_set_callback_ = callback;
   return existing_callback;
@@ -832,6 +867,13 @@ NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callb
 void
 NodeParameters::register_param_change_callback(ParametersCallbackFunction callback)
 {
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  if (!parameter_modification_enabled_) {
+    throw rclcpp::exceptions::ParameterModifiedInCallbackException(
+            "cannot modify a parameter from within set callback");
+  }
+
   if (on_parameters_set_callback_) {
     RCLCPP_WARN(
       node_logging_->get_logger(),

rclcpp/test/test_node.cpp

@@ -2148,3 +2148,183 @@ TEST_F(TestNode, get_parameter_types_undeclared_parameters_allowed) {
     EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET);
   }
 }
+
+// test that it is possible to call get_parameter within the set_callback
+TEST_F(TestNode, set_on_parameters_set_callback_get_parameter) {
+  auto node = std::make_shared<rclcpp::Node>("test_set_callback_get_parameter_node"_unq);
+
+  int intval = node->declare_parameter("intparam", 42);
+  EXPECT_EQ(intval, 42);
+  double floatval = node->declare_parameter("floatparam", 5.4);
+  EXPECT_EQ(floatval, 5.4);
+
+  double floatout;
+  RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);});    // always reset
+  auto on_set_parameters =
+    [&node, &floatout](const std::vector<rclcpp::Parameter> & parameters) {
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = true;
+      if (parameters.size() != 1) {
+        result.successful = false;
+      }
+
+      if (parameters[0].get_value<int>() != 40) {
+        result.successful = false;
+      }
+
+      rclcpp::Parameter floatparam = node->get_parameter("floatparam");
+      if (floatparam.get_value<double>() != 5.4) {
+        result.successful = false;
+      }
+      floatout = floatparam.get_value<double>();
+
+      return result;
+    };
+
+  EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr);
+  ASSERT_NO_THROW(node->set_parameter({"intparam", 40}));
+  ASSERT_EQ(floatout, 5.4);
+}
+
+// test that calling set_parameter inside of a set_callback throws an exception
+TEST_F(TestNode, set_on_parameters_set_callback_set_parameter) {
+  auto node = std::make_shared<rclcpp::Node>("test_set_callback_set_parameter_node"_unq);
+
+  int intval = node->declare_parameter("intparam", 42);
+  EXPECT_EQ(intval, 42);
+  double floatval = node->declare_parameter("floatparam", 5.4);
+  EXPECT_EQ(floatval, 5.4);
+
+  RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);});    // always reset
+  auto on_set_parameters =
+    [&node](const std::vector<rclcpp::Parameter> & parameters) {
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = true;
+      if (parameters.size() != 1) {
+        result.successful = false;
+      }
+
+      if (parameters[0].get_value<int>() != 40) {
+        result.successful = false;
+      }
+
+      // This should throw an exception
+      node->set_parameter({"floatparam", 5.6});
+
+      return result;
+    };
+
+  EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr);
+  EXPECT_THROW({
+    node->set_parameter(rclcpp::Parameter("intparam", 40));
+  }, rclcpp::exceptions::ParameterModifiedInCallbackException);
+}
+
+// test that calling declare_parameter inside of a set_callback throws an exception
+TEST_F(TestNode, set_on_parameters_set_callback_declare_parameter) {
+  auto node = std::make_shared<rclcpp::Node>("test_set_callback_declare_parameter_node"_unq);
+
+  int intval = node->declare_parameter("intparam", 42);
+  EXPECT_EQ(intval, 42);
+  double floatval = node->declare_parameter("floatparam", 5.4);
+  EXPECT_EQ(floatval, 5.4);
+
+  RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);});    // always reset
+  auto on_set_parameters =
+    [&node](const std::vector<rclcpp::Parameter> & parameters) {
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = true;
+      if (parameters.size() != 1) {
+        result.successful = false;
+      }
+
+      if (parameters[0].get_value<int>() != 40) {
+        result.successful = false;
+      }
+
+      // This should throw an exception
+      node->declare_parameter("floatparam2", 5.6);
+
+      return result;
+    };
+
+  EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr);
+  EXPECT_THROW({
+    node->set_parameter(rclcpp::Parameter("intparam", 40));
+  }, rclcpp::exceptions::ParameterModifiedInCallbackException);
+}
+
+// test that calling undeclare_parameter inside a set_callback throws an exception
+TEST_F(TestNode, set_on_parameters_set_callback_undeclare_parameter) {
+  auto node = std::make_shared<rclcpp::Node>("test_set_callback_undeclare_parameter_node"_unq);
+
+  int intval = node->declare_parameter("intparam", 42);
+  EXPECT_EQ(intval, 42);
+  double floatval = node->declare_parameter("floatparam", 5.4);
+  EXPECT_EQ(floatval, 5.4);
+
+  RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);});    // always reset
+  auto on_set_parameters =
+    [&node](const std::vector<rclcpp::Parameter> & parameters) {
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = true;
+      if (parameters.size() != 1) {
+        result.successful = false;
+      }
+
+      if (parameters[0].get_value<int>() != 40) {
+        result.successful = false;
+      }
+
+      // This should throw an exception
+      node->undeclare_parameter("floatparam");
+
+      return result;
+    };
+
+  EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr);
+  EXPECT_THROW({
+    node->set_parameter(rclcpp::Parameter("intparam", 40));
+  }, rclcpp::exceptions::ParameterModifiedInCallbackException);
+}
+
+// test that calling set_on_parameters_set_callback from a set_callback throws an exception
+TEST_F(TestNode, set_on_parameters_set_callback_set_on_parameters_set_callback) {
+  auto node = std::make_shared<rclcpp::Node>("test_set_callback_set_callback_node"_unq);
+
+  int intval = node->declare_parameter("intparam", 42);
+  EXPECT_EQ(intval, 42);
+  double floatval = node->declare_parameter("floatparam", 5.4);
+  EXPECT_EQ(floatval, 5.4);
+
+  RCLCPP_SCOPE_EXIT({node->set_on_parameters_set_callback(nullptr);});    // always reset
+  auto on_set_parameters =
+    [&node](const std::vector<rclcpp::Parameter> & parameters) {
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = true;
+      if (parameters.size() != 1) {
+        result.successful = false;
+      }
+
+      if (parameters[0].get_value<int>() != 40) {
+        result.successful = false;
+      }
+
+      auto bad_parameters =
+        [](const std::vector<rclcpp::Parameter> & parameters) {
+          (void)parameters;
+          rcl_interfaces::msg::SetParametersResult result;
+          return result;
+        };
+
+      // This should throw an exception
+      node->set_on_parameters_set_callback(bad_parameters);
+
+      return result;
+    };
+
+  EXPECT_EQ(node->set_on_parameters_set_callback(on_set_parameters), nullptr);
+  EXPECT_THROW({
+    node->set_parameter(rclcpp::Parameter("intparam", 40));
+  }, rclcpp::exceptions::ParameterModifiedInCallbackException);
+}