Adding API to copy all parameters from one node to another (#2304)

Signed-off-by: stevemacenski <stevenmacenski@gmail.com>

rclcpp/include/rclcpp/copy_all_parameter_values.hpp

@@ -0,0 +1,82 @@
+// Copyright 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_
+#define RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_
+
+#include <string>
+#include <vector>
+
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/logger.hpp"
+#include "rclcpp/logging.hpp"
+
+namespace rclcpp
+{
+
+/**
+ * Copy all parameters from one source node to another destination node.
+ * May throw exceptions if parameters from source are uninitialized or undeclared.
+ * \param source Node to copy parameters from
+ * \param destination Node to copy parameters to
+ * \param override_existing_params Default false. Whether to override existing destination params
+ * if both the source and destination contain the same parameter.
+ */
+template<typename NodeT1, typename NodeT2>
+void
+copy_all_parameter_values(
+  const NodeT1 & source, const NodeT2 & destination, const bool override_existing_params = false)
+{
+  using Parameters = std::vector<rclcpp::Parameter>;
+  using Descriptions = std::vector<rcl_interfaces::msg::ParameterDescriptor>;
+  auto source_params = source->get_node_parameters_interface();
+  auto dest_params = destination->get_node_parameters_interface();
+  rclcpp::Logger logger = destination->get_node_logging_interface()->get_logger();
+
+  std::vector<std::string> param_names = source_params->list_parameters({}, 0).names;
+  Parameters params = source_params->get_parameters(param_names);
+  Descriptions descriptions = source_params->describe_parameters(param_names);
+
+  for (unsigned int idx = 0; idx != params.size(); idx++) {
+    if (!dest_params->has_parameter(params[idx].get_name())) {
+      dest_params->declare_parameter(
+        params[idx].get_name(), params[idx].get_parameter_value(), descriptions[idx]);
+    } else if (override_existing_params) {
+      try {
+        rcl_interfaces::msg::SetParametersResult result =
+          dest_params->set_parameters_atomically({params[idx]});
+        if (!result.successful) {
+          // Parameter update rejected or read-only
+          RCLCPP_WARN(
+            logger,
+            "Unable to set parameter (%s): %s!",
+            params[idx].get_name().c_str(), result.reason.c_str());
+        }
+      } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+        RCLCPP_WARN(
+          logger,
+          "Unable to set parameter (%s): incompatable parameter type (%s)!",
+          params[idx].get_name().c_str(), e.what());
+      }
+    }
+  }
+}
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_

rclcpp/include/rclcpp/rclcpp.hpp

@@ -54,6 +54,7 @@
  *   - rclcpp::ParameterValue
  *   - rclcpp::AsyncParametersClient
  *   - rclcpp::SyncParametersClient
+ *   - rclcpp::copy_all_parameter_values()
  *   - rclcpp/parameter.hpp
  *   - rclcpp/parameter_value.hpp
  *   - rclcpp/parameter_client.hpp
@@ -164,6 +165,7 @@
 #include <csignal>
 #include <memory>
 
+#include "rclcpp/copy_all_parameter_values.hpp"
 #include "rclcpp/executors.hpp"
 #include "rclcpp/guard_condition.hpp"
 #include "rclcpp/logging.hpp"

rclcpp/test/rclcpp/CMakeLists.txt

@@ -75,6 +75,13 @@ if(TARGET test_client)
   )
   target_link_libraries(test_client ${PROJECT_NAME} mimick)
 endif()
+ament_add_gtest(test_copy_all_parameter_values test_copy_all_parameter_values.cpp)
+if(TARGET test_copy_all_parameter_values)
+  ament_target_dependencies(test_copy_all_parameter_values
+    "rcl_interfaces"
+  )
+  target_link_libraries(test_copy_all_parameter_values ${PROJECT_NAME})
+endif()
 ament_add_gtest(test_create_timer test_create_timer.cpp)
 if(TARGET test_create_timer)
   ament_target_dependencies(test_create_timer

rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp

@@ -0,0 +1,88 @@
+// Copyright 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include "rclcpp/copy_all_parameter_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class TestNode : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  static void TearDownTestCase()
+  {
+    rclcpp::shutdown();
+  }
+};
+
+TEST_F(TestNode, TestParamCopying)
+{
+  auto node1 = std::make_shared<rclcpp::Node>("test_node1");
+  auto node2 = std::make_shared<rclcpp::Node>("test_node2");
+
+  // Tests for (1) multiple types, (2) recursion, (3) overriding values
+  node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1"))));
+  node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123));
+  node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar"))));
+  node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve"))));
+  node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2"))));
+
+  // Show Node2 is empty of Node1's parameters, but contains its own
+  EXPECT_FALSE(node2->has_parameter("Foo1"));
+  EXPECT_FALSE(node2->has_parameter("Foo2"));
+  EXPECT_FALSE(node2->has_parameter("Foo.bar"));
+  EXPECT_TRUE(node2->has_parameter("Foo"));
+  EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2"));
+
+  bool override = false;
+  rclcpp::copy_all_parameter_values(node1, node2, override);
+
+  // Test new parameters exist, of expected value, and original param is not overridden
+  EXPECT_TRUE(node2->has_parameter("Foo1"));
+  EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1"));
+  EXPECT_TRUE(node2->has_parameter("Foo2"));
+  EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123);
+  EXPECT_TRUE(node2->has_parameter("Foo.bar"));
+  EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve"));
+  EXPECT_TRUE(node2->has_parameter("Foo"));
+  EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2"));
+
+  // Test if parameter overrides are permissible that Node2's value is overridden
+  override = true;
+  rclcpp::copy_all_parameter_values(node1, node2, override);
+  EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("bar"));
+}
+
+TEST_F(TestNode, TestParamCopyingExceptions)
+{
+  auto node1 = std::make_shared<rclcpp::Node>("test_node1");
+  auto node2 = std::make_shared<rclcpp::Node>("test_node2");
+
+  // Tests for Parameter value conflicts handled
+  node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar"))));
+  node2->declare_parameter("Foo", rclcpp::ParameterValue(0.123));
+
+  bool override = true;
+  EXPECT_NO_THROW(
+    rclcpp::copy_all_parameter_values(node1, node2, override));
+
+  // Tests for Parameter read-only handled
+  node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar"))));
+  node2->declare_parameter("Foo1", rclcpp::ParameterValue(0.123));
+  EXPECT_NO_THROW(rclcpp::copy_all_parameter_values(node1, node2, override));
+}