Add force set parameter functionality

rclcpp/include/rclcpp/node.hpp

@@ -562,13 +562,14 @@ class Node : public std::enable_shared_from_this<Node>
    * 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<Node>
    * 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<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters, bool force = false);
 
   /// Set one or more parameters, all at once.
   /**
@@ -634,13 +636,16 @@ class Node : public std::enable_shared_from_this<Node>
    * 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<rclcpp::Parameter> & parameters);
+  set_parameters_atomically(
+    const std::vector<rclcpp::Parameter> & parameters,
+    bool force = false);
 
   /// Return the parameter by the given name.
   /**

rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp

@@ -151,12 +151,14 @@ class NodeParameters : public NodeParametersInterface
   RCLCPP_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
   set_parameters(
-    const std::vector<rclcpp::Parameter> & parameters) override;
+    const std::vector<rclcpp::Parameter> & parameters,
+    bool force) override;
 
   RCLCPP_PUBLIC
   rcl_interfaces::msg::SetParametersResult
   set_parameters_atomically(
-    const std::vector<rclcpp::Parameter> & parameters) override;
+    const std::vector<rclcpp::Parameter> & parameters,
+    bool force) override;
 
   RCLCPP_PUBLIC
   std::vector<rclcpp::Parameter>

rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp

@@ -128,7 +128,7 @@ class NodeParametersInterface
   RCLCPP_PUBLIC
   virtual
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::Parameter> & parameters) = 0;
+  set_parameters(const std::vector<rclcpp::Parameter> & 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<rclcpp::Parameter> & parameters) = 0;
+  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters, bool force) = 0;
 
   /// Get descriptions of parameters given their names.
   /*

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<rcl_interfaces::msg::SetParametersResult>
-Node::set_parameters(const std::vector<rclcpp::Parameter> & parameters)
+Node::set_parameters(const std::vector<rclcpp::Parameter> & 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<rclcpp::Parameter> & parameters)
+Node::set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters, bool force)
 {
-  return node_parameters_->set_parameters_atomically(parameters);
+  return node_parameters_->set_parameters_atomically(parameters, force);
 }
 
 rclcpp::Parameter

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -296,20 +296,28 @@ __set_parameters_atomically_common(
   std::map<std::string, rclcpp::node_interfaces::ParameterInfo> & 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<rcl_interfaces::msg::SetParametersResult>
-NodeParameters::set_parameters(const std::vector<rclcpp::Parameter> & parameters)
+NodeParameters::set_parameters(
+  const std::vector<rclcpp::Parameter> & parameters,
+  bool force)
 {
   std::vector<rcl_interfaces::msg::SetParametersResult> 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<rclcpp::Parameter> & parameters)
+NodeParameters::set_parameters_atomically(
+  const std::vector<rclcpp::Parameter> & parameters,
+  bool force)
 {
   std::lock_guard<std::recursive_mutex> lock(mutex_);
 
@@ -632,7 +644,7 @@ NodeParameters::set_parameters_atomically(const std::vector<rclcpp::Parameter> &
     }
 
     // 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<rclcpp::Parameter> &
     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<rclcpp::Parameter> &
     // 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) {

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(

rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp

@@ -153,19 +153,26 @@ 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);
   EXPECT_STREQ(
     "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);

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<rclcpp::Node>(
+    "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<int>(), 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<int>(), 42);
+
+    // Forced methods will succeed
+    EXPECT_TRUE(node->set_parameter({name, 43}, true).successful);
+    EXPECT_EQ(node->get_parameter(name).get_value<int>(), 43);
+
+    EXPECT_TRUE(node->set_parameters_atomically({{name, 44}}, true).successful);
+    EXPECT_EQ(node->get_parameter(name).get_value<int>(), 44);
+
+    EXPECT_TRUE(node->set_parameters({{name, 45}}, true)[0].successful);
+    EXPECT_EQ(node->get_parameter(name).get_value<int>(), 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<int>(), 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<int>(), 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<std::string>(), "this is a string");
+
+    EXPECT_TRUE(node->set_parameters_atomically({{name, 44}}, true).successful);
+    EXPECT_EQ(node->get_parameter(name).get_value<int>(), 44);
+
+    EXPECT_TRUE(node->set_parameters({{name, "this is another string"}}, true)[0].successful);
+    EXPECT_EQ(node->get_parameter(name).get_value<std::string>(), "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<rclcpp::Node>(

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -419,23 +419,25 @@ 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.
   /**
    * \sa rclcpp::Node::set_parameters
    */
   RCLCPP_LIFECYCLE_PUBLIC
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(const std::vector<rclcpp::Parameter> & parameters);
+  set_parameters(const std::vector<rclcpp::Parameter> & parameters, bool force = false);
 
   /// Set one or more parameters, all at once.
   /**
    * \sa rclcpp::Node::set_parameters_atomically
    */
   RCLCPP_LIFECYCLE_PUBLIC
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(const std::vector<rclcpp::Parameter> & parameters);
+  set_parameters_atomically(
+    const std::vector<rclcpp::Parameter> & parameters,
+    bool force = false);
 
   /// Return the parameter by the given name.
   /**

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<rcl_interfaces::msg::SetParametersResult>
 LifecycleNode::set_parameters(
-  const std::vector<rclcpp::Parameter> & parameters)
+  const std::vector<rclcpp::Parameter> & 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<rclcpp::Parameter> & parameters)
+  const std::vector<rclcpp::Parameter> & parameters,
+  bool force)
 {
-  return node_parameters_->set_parameters_atomically(parameters);
+  return node_parameters_->set_parameters_atomically(parameters, force);
 }
 
 std::vector<rclcpp::Parameter>