diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7e5f402f8a..048f7c4691 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -562,13 +562,14 @@ class Node : public std::enable_shared_from_this * deleted. * * \param[in] parameter The parameter to be set. + * \param[in] force Whether to ignore immutable parameter constraints. * \return The result of the set action. * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter * has not been declared and undeclared parameters are not allowed. */ RCLCPP_PUBLIC rcl_interfaces::msg::SetParametersResult - set_parameter(const rclcpp::Parameter & parameter); + set_parameter(const rclcpp::Parameter & parameter, bool force = false); /// Set one or more parameters, one at a time. /** @@ -600,13 +601,14 @@ class Node : public std::enable_shared_from_this * with the type rclcpp::PARAMETER_NOT_SET. * * \param[in] parameters The vector of parameters to be set. + * \param[in] force Whether to ignore immutable parameter constraints. * \return The results for each set action as a vector. * \throws rclcpp::exceptions::ParameterNotDeclaredException if any parameter * has not been declared and undeclared parameters are not allowed. */ RCLCPP_PUBLIC std::vector - set_parameters(const std::vector & parameters); + set_parameters(const std::vector & parameters, bool force = false); /// Set one or more parameters, all at once. /** @@ -634,13 +636,16 @@ class Node : public std::enable_shared_from_this * with the type rclcpp::PARAMETER_NOT_SET. * * \param[in] parameters The vector of parameters to be set. + * \param[in] force Whether to ignore immutable parameter constraints. * \return The aggregate result of setting all the parameters atomically. * \throws rclcpp::exceptions::ParameterNotDeclaredException if any parameter * has not been declared and undeclared parameters are not allowed. */ RCLCPP_PUBLIC rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters); + set_parameters_atomically( + const std::vector & parameters, + bool force = false); /// Return the parameter by the given name. /** diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 552fbc6979..64ffd71780 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -151,12 +151,14 @@ class NodeParameters : public NodeParametersInterface RCLCPP_PUBLIC std::vector set_parameters( - const std::vector & parameters) override; + const std::vector & parameters, + bool force) override; RCLCPP_PUBLIC rcl_interfaces::msg::SetParametersResult set_parameters_atomically( - const std::vector & parameters) override; + const std::vector & parameters, + bool force) override; RCLCPP_PUBLIC std::vector diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index d9981516c7..3f32442d59 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -128,7 +128,7 @@ class NodeParametersInterface RCLCPP_PUBLIC virtual std::vector - set_parameters(const std::vector & parameters) = 0; + set_parameters(const std::vector & parameters, bool force) = 0; /// Set one or more parameters, all at once. /** @@ -137,8 +137,7 @@ class NodeParametersInterface RCLCPP_PUBLIC virtual rcl_interfaces::msg::SetParametersResult - set_parameters_atomically( - const std::vector & parameters) = 0; + set_parameters_atomically(const std::vector & parameters, bool force) = 0; /// Get descriptions of parameters given their names. /* diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 5d1f9a3077..95475d5a76 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -367,21 +367,21 @@ Node::has_parameter(const std::string & name) const } rcl_interfaces::msg::SetParametersResult -Node::set_parameter(const rclcpp::Parameter & parameter) +Node::set_parameter(const rclcpp::Parameter & parameter, bool force) { - return this->set_parameters_atomically({parameter}); + return this->set_parameters_atomically({parameter}, force); } std::vector -Node::set_parameters(const std::vector & parameters) +Node::set_parameters(const std::vector & parameters, bool force) { - return node_parameters_->set_parameters(parameters); + return node_parameters_->set_parameters(parameters, force); } rcl_interfaces::msg::SetParametersResult -Node::set_parameters_atomically(const std::vector & parameters) +Node::set_parameters_atomically(const std::vector & parameters, bool force) { - return node_parameters_->set_parameters_atomically(parameters); + return node_parameters_->set_parameters_atomically(parameters, force); } rclcpp::Parameter diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 92d42bd8e7..45df297440 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -296,20 +296,28 @@ __set_parameters_atomically_common( std::map & parameter_infos, CallbacksContainerType & callback_container, const OnParametersSetCallbackType & callback, - bool allow_undeclared = false) + bool allow_undeclared = false, + bool force = false) { - // Check if the value being set complies with the descriptor. - rcl_interfaces::msg::SetParametersResult result = __check_parameters( - parameter_infos, parameters, allow_undeclared); - if (!result.successful) { - return result; - } - // Call the user callback to see if the new value(s) are allowed. - result = - __call_on_parameters_set_callbacks(parameters, callback_container, callback); - if (!result.successful) { - return result; + rcl_interfaces::msg::SetParametersResult result; + if (force) { + // If the force flag is set, we skip checking descriptor and user callbacks + result.successful = true; + } else { + // Check if the value being set complies with the descriptor. + result = __check_parameters( + parameter_infos, parameters, allow_undeclared); + if (!result.successful) { + return result; + } + // Call the user callback to see if the new value(s) are allowed. + result = + __call_on_parameters_set_callbacks(parameters, callback_container, callback); + if (!result.successful) { + return result; + } } + // If accepted, actually set the values. if (result.successful) { for (size_t i = 0; i < parameters.size(); ++i) { @@ -567,13 +575,15 @@ NodeParameters::has_parameter(const std::string & name) const } std::vector -NodeParameters::set_parameters(const std::vector & parameters) +NodeParameters::set_parameters( + const std::vector & parameters, + bool force) { std::vector results; results.reserve(parameters.size()); for (const auto & p : parameters) { - auto result = set_parameters_atomically({{p}}); + auto result = set_parameters_atomically({{p}}, force); results.push_back(result); } @@ -593,7 +603,9 @@ __find_parameter_by_name( } rcl_interfaces::msg::SetParametersResult -NodeParameters::set_parameters_atomically(const std::vector & parameters) +NodeParameters::set_parameters_atomically( + const std::vector & parameters, + bool force) { std::lock_guard lock(mutex_); @@ -632,7 +644,7 @@ NodeParameters::set_parameters_atomically(const std::vector & } // Check to see if it is read-only. - if (parameter_info->second.descriptor.read_only) { + if (parameter_info->second.descriptor.read_only && !force) { result.successful = false; result.reason = "parameter '" + name + "' cannot be set because it is read-only"; return result; @@ -706,8 +718,8 @@ NodeParameters::set_parameters_atomically(const std::vector & if (rclcpp::PARAMETER_NOT_SET == parameter.get_type()) { auto it = parameters_.find(parameter.get_name()); if (it != parameters_.end() && rclcpp::PARAMETER_NOT_SET != it->second.value.get_type()) { - if (!it->second.descriptor.dynamic_typing) { - result.reason = "cannot undeclare an statically typed parameter"; + if (!it->second.descriptor.dynamic_typing && !force) { + result.reason = "cannot undeclare a statically typed parameter"; result.successful = false; return result; } @@ -727,7 +739,10 @@ NodeParameters::set_parameters_atomically(const std::vector & // These callbacks are called once. When a callback returns an unsuccessful result, // the remaining aren't called. on_parameters_set_callback_, - allow_undeclared_); // allow undeclared + // allow undeclared + allow_undeclared_, + // ignore constraints and force set the new value + force); // If not successful, then stop here. if (!result.successful) { diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 5c30917499..78cec0572c 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -87,7 +87,8 @@ ParameterService::ParameterService( for (auto & p : request->parameters) { try { result = node_params->set_parameters_atomically( - {rclcpp::Parameter::from_parameter_msg(p)}); + {rclcpp::Parameter::from_parameter_msg(p)}, + false); } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what()); result.successful = false; @@ -114,7 +115,7 @@ ParameterService::ParameterService( return rclcpp::Parameter::from_parameter_msg(p); }); try { - auto result = node_params->set_parameters_atomically(pvariants); + auto result = node_params->set_parameters_atomically(pvariants, false); response->result = result; } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { RCLCPP_DEBUG( diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 31b755b4a7..3fdc0720c0 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -153,7 +153,7 @@ TEST_F(TestNodeParameters, set_parameters) { rclcpp::Parameter("bool_parameter", true), rclcpp::Parameter("read_only_parameter", 42), }; - auto result = node_parameters->set_parameters(parameters); + auto result = node_parameters->set_parameters(parameters, false); ASSERT_EQ(parameters.size(), result.size()); EXPECT_TRUE(result[0].successful); EXPECT_FALSE(result[1].successful); @@ -161,11 +161,18 @@ TEST_F(TestNodeParameters, set_parameters) { "parameter 'read_only_parameter' cannot be set because it is read-only", result[1].reason.c_str()); + result = node_parameters->set_parameters({rclcpp::Parameter("read_only_parameter", 55)}, true); + ASSERT_EQ(1u, result.size()); + EXPECT_TRUE(result[0].successful); + RCLCPP_EXPECT_THROW_EQ( - node_parameters->set_parameters({rclcpp::Parameter("", true)}), + node_parameters->set_parameters({rclcpp::Parameter("", true)}, false), rclcpp::exceptions::InvalidParametersException("parameter name must not be empty")); - result = node_parameters->set_parameters({rclcpp::Parameter("undeclared_parameter", 3.14159)}); + result = node_parameters->set_parameters( + {rclcpp::Parameter( + "undeclared_parameter", + 3.14159)}, false); ASSERT_EQ(1u, result.size()); EXPECT_TRUE(result[0].successful); } @@ -188,7 +195,7 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) { }; auto handle = node_parameters->add_on_set_parameters_callback(callback); - auto result = node_parameters->set_parameters(parameters); + auto result = node_parameters->set_parameters(parameters, false); ASSERT_EQ(1u, result.size()); EXPECT_FALSE(result[0].successful); EXPECT_EQ(reason, result[0].reason); diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index e034af16b2..1a1c6e3199 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -1874,6 +1874,64 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { } } +// test force_set_parameters +TEST_F(TestNode, force_set_parameters) { + auto node = std::make_shared( + "force_set_parameters"_unq); + { + // Read only parameter + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name, 42, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + + // Regular methods will fail + EXPECT_FALSE(node->set_parameter({name, 43}, false).successful); + EXPECT_FALSE(node->set_parameters_atomically({{name, 43}}, false).successful); + EXPECT_FALSE(node->set_parameters({{name, 43}}, false)[0].successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + + // Forced methods will succeed + EXPECT_TRUE(node->set_parameter({name, 43}, true).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 43); + + EXPECT_TRUE(node->set_parameters_atomically({{name, 44}}, true).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 44); + + EXPECT_TRUE(node->set_parameters({{name, 45}}, true)[0].successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 45); + } + { + // Static type parameter + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = false; + node->declare_parameter(name, 42, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + + // Regular methods will fail + EXPECT_FALSE(node->set_parameter({name, "this is a string"}, false).successful); + EXPECT_FALSE(node->set_parameters_atomically({{name, "this is a string"}}, false).successful); + EXPECT_FALSE(node->set_parameters({{name, "this is a string"}}, false)[0].successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + + // Forced methods will succeed + EXPECT_TRUE(node->set_parameter({name, "this is a string"}, true).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), "this is a string"); + + EXPECT_TRUE(node->set_parameters_atomically({{name, 44}}, true).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 44); + + EXPECT_TRUE(node->set_parameters({{name, "this is another string"}}, true)[0].successful); + EXPECT_EQ(node->get_parameter(name).get_value(), "this is another string"); + } +} + // test get_parameter with undeclared not allowed TEST_F(TestNode, get_parameter_undeclared_parameters_not_allowed) { auto node = std::make_shared( diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 4442304c0c..b598e0272a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -419,7 +419,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC rcl_interfaces::msg::SetParametersResult - set_parameter(const rclcpp::Parameter & parameter); + set_parameter(const rclcpp::Parameter & parameter, bool force = false); /// Set one or more parameters, one at a time. /** @@ -427,7 +427,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC std::vector - set_parameters(const std::vector & parameters); + set_parameters(const std::vector & parameters, bool force = false); /// Set one or more parameters, all at once. /** @@ -435,7 +435,9 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters); + set_parameters_atomically( + const std::vector & parameters, + bool force = false); /// Return the parameter by the given name. /** diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index d2bd49bcf7..a1351804c8 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -227,23 +227,25 @@ LifecycleNode::has_parameter(const std::string & name) const } rcl_interfaces::msg::SetParametersResult -LifecycleNode::set_parameter(const rclcpp::Parameter & parameter) +LifecycleNode::set_parameter(const rclcpp::Parameter & parameter, bool force) { - return this->set_parameters_atomically({parameter}); + return this->set_parameters_atomically({parameter}, force); } std::vector LifecycleNode::set_parameters( - const std::vector & parameters) + const std::vector & parameters, + bool force) { - return node_parameters_->set_parameters(parameters); + return node_parameters_->set_parameters(parameters, force); } rcl_interfaces::msg::SetParametersResult LifecycleNode::set_parameters_atomically( - const std::vector & parameters) + const std::vector & parameters, + bool force) { - return node_parameters_->set_parameters_atomically(parameters); + return node_parameters_->set_parameters_atomically(parameters, force); } std::vector