Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -135,7 +135,7 @@ bool BtActionServer<ActionT>::on_configure()
node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
nav2_util::declare_parameter_if_not_declared(
node, "transform_tolerance", rclcpp::ParameterValue(0.1));
nav2_util::copy_all_parameters(node, client_node_);
rclcpp::copy_all_parameter_values(node, client_node_);

// set the timeout in seconds for the action server to discard goal handles if not finished
double action_server_result_timeout;
Expand Down
19 changes: 0 additions & 19 deletions nav2_util/include/nav2_util/node_utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -153,25 +153,6 @@ std::string get_plugin_type_param(
return plugin_type;
}

/**
* @brief A method to copy all parameters from one node (parent) to another (child).
* May throw parameter exceptions in error conditions
* @param parent Node to copy parameters from
* @param child Node to copy parameters to
*/
template<typename NodeT1, typename NodeT2>
void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child)
{
using Parameters = std::vector<rclcpp::Parameter>;
std::vector<std::string> param_names = parent->list_parameters({}, 0).names;
Parameters params = parent->get_parameters(param_names);
for (Parameters::const_iterator iter = params.begin(); iter != params.end(); ++iter) {
if (!child->has_parameter(iter->get_name())) {
child->declare_parameter(iter->get_name(), iter->get_parameter_value());
}
}
}

/**
* @brief Sets the caller thread to have a soft-realtime prioritization by
* increasing the priority level of the host thread.
Expand Down
32 changes: 0 additions & 32 deletions nav2_util/test/test_node_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -94,35 +94,3 @@ TEST(GetPluginTypeParam, GetPluginTypeParam)
ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar");
EXPECT_THROW(get_plugin_type_param(node, "Waldo"), std::runtime_error);
}

TEST(TestParamCopying, 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"));

nav2_util::copy_all_parameters(node1, node2);

// 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"));
}