diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index c65df54..6cbb6b8 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/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 diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index a070868..e752404 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -928,6 +928,14 @@ public: * * 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. diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 149bcbd..5c9cb69 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/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 @@ public: 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; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 9c951c3..f48cd5e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/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 lock(mutex_); + std::lock_guard 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 lock(mutex_); + std::lock_guard 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 lock(mutex_); + std::lock_guard 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 & parameters) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); + + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); rcl_interfaces::msg::SetParametersResult result; @@ -575,7 +581,7 @@ NodeParameters::set_parameters_atomically(const std::vector & } } - // 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 & std::vector NodeParameters::get_parameters(const std::vector & names) const { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); std::vector results; results.reserve(names.size()); @@ -683,7 +689,7 @@ NodeParameters::get_parameter( const std::string & name, rclcpp::Parameter & parameter) const { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); auto param_iter = parameters_.find(name); if ( @@ -702,7 +708,7 @@ NodeParameters::get_parameters_by_prefix( const std::string & prefix, std::map & parameters) const { - std::lock_guard lock(mutex_); + std::lock_guard 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 NodeParameters::describe_parameters(const std::vector & names) const { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); std::vector results; results.reserve(names.size()); @@ -749,7 +755,7 @@ NodeParameters::describe_parameters(const std::vector & names) cons std::vector NodeParameters::get_parameter_types(const std::vector & names) const { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); std::vector results; results.reserve(names.size()); @@ -775,7 +781,7 @@ NodeParameters::get_parameter_types(const std::vector & names) cons rcl_interfaces::msg::ListParametersResult NodeParameters::list_parameters(const std::vector & prefixes, uint64_t depth) const { - std::lock_guard lock(mutex_); + std::lock_guard 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 & prefixes, uint6 NodeParameters::OnParametersSetCallbackType NodeParameters::set_on_parameters_set_callback(OnParametersSetCallbackType callback) { + std::lock_guard 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 lock(mutex_); + + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + if (on_parameters_set_callback_) { RCLCPP_WARN( node_logging_->get_logger(), diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp index 862bcb6..414de0e 100644 --- a/rclcpp/test/test_node.cpp +++ b/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("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 & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } + + if (parameters[0].get_value() != 40) { + result.successful = false; + } + + rclcpp::Parameter floatparam = node->get_parameter("floatparam"); + if (floatparam.get_value() != 5.4) { + result.successful = false; + } + floatout = floatparam.get_value(); + + 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("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 & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } + + if (parameters[0].get_value() != 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("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 & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } + + if (parameters[0].get_value() != 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("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 & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } + + if (parameters[0].get_value() != 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("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 & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (parameters.size() != 1) { + result.successful = false; + } + + if (parameters[0].get_value() != 40) { + result.successful = false; + } + + auto bad_parameters = + [](const std::vector & 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); +}