diff --git a/rclcpp/include/rclcpp/copy_all_parameter_values.hpp b/rclcpp/include/rclcpp/copy_all_parameter_values.hpp new file mode 100644 index 0000000000..cc61b621e1 --- /dev/null +++ b/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 +#include + +#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 +void +copy_all_parameter_values( + const NodeT1 & source, const NodeT2 & destination, const bool override_existing_params = false) +{ + using Parameters = std::vector; + using Descriptions = std::vector; + 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 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_ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index ef587578e2..1d5b9113b5 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/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 #include +#include "rclcpp/copy_all_parameter_values.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/logging.hpp" diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 159f990f32..afa12cd24d 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/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 diff --git a/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp b/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp new file mode 100644 index 0000000000..e3020efd1e --- /dev/null +++ b/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 +#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("test_node1"); + auto node2 = std::make_shared("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("test_node1"); + auto node2 = std::make_shared("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)); +}