Skip to content

Commit

Permalink
Adding API to copy all parameters from one node to another (#2304)
Browse files Browse the repository at this point in the history
Signed-off-by: stevemacenski <[email protected]>
  • Loading branch information
SteveMacenski authored Oct 5, 2023
1 parent 0644f22 commit 9019a8d
Show file tree
Hide file tree
Showing 4 changed files with 179 additions and 0 deletions.
82 changes: 82 additions & 0 deletions rclcpp/include/rclcpp/copy_all_parameter_values.hpp
Original file line number Diff line number Diff line change
@@ -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_
2 changes: 2 additions & 0 deletions rclcpp/include/rclcpp/rclcpp.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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"
Expand Down
7 changes: 7 additions & 0 deletions rclcpp/test/rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
88 changes: 88 additions & 0 deletions rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp
Original file line number Diff line number Diff line change
@@ -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));
}

0 comments on commit 9019a8d

Please sign in to comment.