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.hpp

@@ -928,6 +928,14 @@ class Node : public std::enable_shared_from_this<Node>
    *
    * Some constraints like read_only are enforced before the callback is called.
    *
+   * The callback may introspect other already set parameters (by calling any
+   * of the {get,list,describe}_parameter() methods), but may *not* modify
+   * other parameters (by calling any of the {set,declare}_parameter() methods)
+   * or modify the registered callback itself (by calling the
+   * set_on_parameters_set_callback() method).  If a callback tries to do any
+   * of the latter things,
+   * rclcpp::exceptions::ParameterModifiedInCallbackException will be thrown.
+   *
    * There may only be one callback set at a time, so the previously set
    * callback is returned when this method is used, or nullptr will be returned
    * if no callback was previously set.

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -50,6 +50,30 @@ struct ParameterInfo
   rcl_interfaces::msg::ParameterDescriptor descriptor;
 };
 
+// Internal RAII-style guard for mutation recursion
+class ParameterMutationRecursionGuard
+{
+public:
+  explicit ParameterMutationRecursionGuard(bool & allow_mod)
+  : allow_modification_(allow_mod)
+  {
+    if (!allow_modification_) {
+      throw rclcpp::exceptions::ParameterModifiedInCallbackException(
+              "cannot set or declare a parameter, or change the callback from within set callback");
+    }
+
+    allow_modification_ = false;
+  }
+
+  ~ParameterMutationRecursionGuard()
+  {
+    allow_modification_ = true;
+  }
+
+private:
+  bool & allow_modification_;
+};
+
 /// Implementation of the NodeParameters part of the Node API.
 class NodeParameters : public NodeParametersInterface
 {
@@ -149,7 +173,12 @@ class NodeParameters : public NodeParametersInterface
 private:
   RCLCPP_DISABLE_COPY(NodeParameters)
 
-  mutable std::mutex mutex_;
+  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;
 

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -374,7 +374,9 @@ NodeParameters::declare_parameter(
   const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor,
   bool ignore_override)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
 
   // TODO(sloretz) parameter name validation
   if (name.empty()) {
@@ -415,7 +417,9 @@ NodeParameters::declare_parameter(
 void
 NodeParameters::undeclare_parameter(const std::string & name)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
 
   auto parameter_info = parameters_.find(name);
   if (parameter_info == parameters_.end()) {
@@ -434,7 +438,7 @@ NodeParameters::undeclare_parameter(const std::string & name)
 bool
 NodeParameters::has_parameter(const std::string & name) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
 
   return __lockless_has_parameter(parameters_, name);
 }
@@ -468,7 +472,9 @@ __find_parameter_by_name(
 rcl_interfaces::msg::SetParametersResult
 NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
 
   rcl_interfaces::msg::SetParametersResult result;
 
@@ -575,7 +581,7 @@ 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.
   result = __set_parameters_atomically_common(
     // either the original parameters given by the user, or ones updated with initial values
     *parameters_to_be_set,
@@ -644,7 +650,7 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
 std::vector<rclcpp::Parameter>
 NodeParameters::get_parameters(const std::vector<std::string> & names) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
   std::vector<rclcpp::Parameter> results;
   results.reserve(names.size());
 
@@ -683,7 +689,7 @@ NodeParameters::get_parameter(
   const std::string & name,
   rclcpp::Parameter & parameter) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
 
   auto param_iter = parameters_.find(name);
   if (
@@ -702,7 +708,7 @@ NodeParameters::get_parameters_by_prefix(
   const std::string & prefix,
   std::map<std::string, rclcpp::Parameter> & parameters) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
 
   std::string prefix_with_dot = prefix.empty() ? prefix : prefix + ".";
   bool ret = false;
@@ -721,7 +727,7 @@ NodeParameters::get_parameters_by_prefix(
 std::vector<rcl_interfaces::msg::ParameterDescriptor>
 NodeParameters::describe_parameters(const std::vector<std::string> & names) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
   std::vector<rcl_interfaces::msg::ParameterDescriptor> results;
   results.reserve(names.size());
 
@@ -749,7 +755,7 @@ NodeParameters::describe_parameters(const std::vector<std::string> & names) cons
 std::vector<uint8_t>
 NodeParameters::get_parameter_types(const std::vector<std::string> & names) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
   std::vector<uint8_t> results;
   results.reserve(names.size());
 
@@ -775,7 +781,7 @@ NodeParameters::get_parameter_types(const std::vector<std::string> & names) cons
 rcl_interfaces::msg::ListParametersResult
 NodeParameters::list_parameters(const std::vector<std::string> & prefixes, uint64_t depth) const
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
   rcl_interfaces::msg::ListParametersResult result;
 
   // TODO(mikaelarguedas) define parameter separator different from "/" to avoid ambiguity
@@ -817,6 +823,10 @@ 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_);
+
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
   auto existing_callback = on_parameters_set_callback_;
   on_parameters_set_callback_ = callback;
   return existing_callback;
@@ -832,6 +842,10 @@ NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callb
 void
 NodeParameters::register_param_change_callback(ParametersCallbackFunction callback)
 {
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+
+  ParameterMutationRecursionGuard guard(parameter_modification_enabled_);
+
   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);
+
+  int64_t 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);
+
+  int64_t 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);
+
+  int64_t 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);
+
+  int64_t 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);
+
+  int64_t 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);
+}