diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index b160e6bb40..3da0e466bd 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -209,6 +209,10 @@ if(BUILD_TESTING) ) target_link_libraries(test_node_global_args ${PROJECT_NAME}) endif() + ament_add_gtest(test_node_initial_parameters test/test_node_initial_parameters.cpp) + if(TARGET test_node_initial_parameters) + target_link_libraries(test_node_initial_parameters ${PROJECT_NAME}) + endif() ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp) if(TARGET test_parameter_events_filter) target_include_directories(test_parameter_events_filter PUBLIC diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 149b81d0cb..87974ef16b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -85,6 +85,7 @@ class Node : public std::enable_shared_from_this * \param[in] namespace_ Namespace of the node. * \param[in] context The context for the node (usually represents the state of a process). * \param[in] arguments Command line arguments that should apply only to this node. + * \param[in] initial_parameters a list of initial values for parameters on the node. * This can be used to provide remapping rules that only affect one instance. * \param[in] use_global_arguments False to prevent node using arguments passed to the process. * \param[in] use_intra_process_comms True to use the optimized intra-process communication @@ -96,6 +97,7 @@ class Node : public std::enable_shared_from_this const std::string & namespace_, rclcpp::Context::SharedPtr context, const std::vector & arguments, + const std::vector & initial_parameters, bool use_global_arguments = true, bool use_intra_process_comms = false, bool start_parameter_services = true); diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index eb02d97459..b872de0318 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -51,6 +51,7 @@ class NodeParameters : public NodeParametersInterface const node_interfaces::NodeBaseInterface::SharedPtr node_base, const node_interfaces::NodeTopicsInterface::SharedPtr node_topics, const node_interfaces::NodeServicesInterface::SharedPtr node_services, + const std::vector & initial_parameters, bool use_intra_process, bool start_parameter_services); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index cb12a3e414..67b187a3bd 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -44,6 +44,7 @@ Node::Node( namespace_, rclcpp::contexts::default_context::get_global_default_context(), {}, + {}, true, use_intra_process_comms, true) @@ -54,6 +55,7 @@ Node::Node( const std::string & namespace_, rclcpp::Context::SharedPtr context, const std::vector & arguments, + const std::vector & initial_parameters, bool use_global_arguments, bool use_intra_process_comms, bool start_parameter_services) @@ -68,6 +70,7 @@ Node::Node( node_base_, node_topics_, node_services_, + initial_parameters, use_intra_process_comms, start_parameter_services )), diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index fcc7c87f7f..52ac98cdb3 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -31,6 +31,7 @@ NodeParameters::NodeParameters( const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + const std::vector & initial_parameters, bool use_intra_process, bool start_parameter_services) { @@ -50,6 +51,15 @@ NodeParameters::NodeParameters( rmw_qos_profile_parameter_events, use_intra_process, allocator); + + // TODO(sloretz) store initial values and use them when a parameter is created ros2/rclcpp#475 + // Set initial parameter values + if (!initial_parameters.empty()) { + rcl_interfaces::msg::SetParametersResult result = set_parameters_atomically(initial_parameters); + if (!result.successful) { + throw std::runtime_error("Failed to set initial parameters"); + } + } } NodeParameters::~NodeParameters() diff --git a/rclcpp/test/test_node_global_args.cpp b/rclcpp/test/test_node_global_args.cpp index cff72d4245..76b21c1e32 100644 --- a/rclcpp/test/test_node_global_args.cpp +++ b/rclcpp/test/test_node_global_args.cpp @@ -35,28 +35,30 @@ class TestNodeWithGlobalArgs : public ::testing::Test TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) { auto context = rclcpp::contexts::default_context::get_global_default_context(); const std::vector arguments = {"__node:=local_arguments_test"}; + const std::vector initial_values = {}; const bool use_global_arguments = true; const bool use_intra_process = false; auto node = rclcpp::Node::make_shared( - "orig_name", "", context, arguments, use_global_arguments, use_intra_process); + "orig_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process); EXPECT_STREQ("local_arguments_test", node->get_name()); } TEST_F(TestNodeWithGlobalArgs, use_or_ignore_global_arguments) { auto context = rclcpp::contexts::default_context::get_global_default_context(); const std::vector arguments = {}; + const std::vector initial_values = {}; const bool use_intra_process = false; { // Don't use global args const bool use_global_arguments = false; auto node = rclcpp::Node::make_shared( - "orig_name", "", context, arguments, use_global_arguments, use_intra_process); + "orig_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process); EXPECT_STREQ("orig_name", node->get_name()); } { // Do use global args const bool use_global_arguments = true; auto node = rclcpp::Node::make_shared( - "orig_name", "", context, arguments, use_global_arguments, use_intra_process); + "orig_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process); EXPECT_STREQ("global_node_name", node->get_name()); } } diff --git a/rclcpp/test/test_node_initial_parameters.cpp b/rclcpp/test/test_node_initial_parameters.cpp new file mode 100644 index 0000000000..4c7a29ca67 --- /dev/null +++ b/rclcpp/test/test_node_initial_parameters.cpp @@ -0,0 +1,70 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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 +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNodeWithInitialValues : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, NULL); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeWithInitialValues, no_initial_values) { + auto context = rclcpp::contexts::default_context::get_global_default_context(); + const std::vector arguments = {}; + const std::vector initial_values = {}; + const bool use_global_arguments = false; + const bool use_intra_process = false; + auto node = rclcpp::Node::make_shared( + "node_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process); + auto list_params_result = node->list_parameters({}, 0); + EXPECT_EQ(0u, list_params_result.names.size()); +} + +TEST_F(TestNodeWithInitialValues, multiple_initial_values) { + auto context = rclcpp::contexts::default_context::get_global_default_context(); + const std::vector arguments = {}; + const std::vector initial_values = { + rclcpp::Parameter("foo", true), + rclcpp::Parameter("bar", "hello world"), + rclcpp::Parameter("baz", std::vector{3.14, 2.718}) + }; + const bool use_global_arguments = false; + const bool use_intra_process = false; + auto node = rclcpp::Node::make_shared( + "node_name", "", context, arguments, initial_values, use_global_arguments, use_intra_process); + auto list_params_result = node->list_parameters({}, 0); + EXPECT_EQ(3u, list_params_result.names.size()); + EXPECT_TRUE(node->get_parameter("foo").get_value()); + EXPECT_STREQ("hello world", node->get_parameter("bar").get_value().c_str()); + std::vector double_array = node->get_parameter("baz").get_value>(); + ASSERT_EQ(2u, double_array.size()); + EXPECT_DOUBLE_EQ(3.14, double_array.at(0)); + EXPECT_DOUBLE_EQ(2.718, double_array.at(1)); +} diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 695eb8ab6d..e4bf37f5d8 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -89,6 +89,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, * \param[in] node_name Namespace of the node. * \param[in] context The context for the node (usually represents the state of a process). * \param[in] arguments Command line arguments that should apply only to this node. + * \param[in] initial_parameters a list of initial values for parameters on the node. * This can be used to provide remapping rules that only affect one instance. * \param[in] use_global_arguments False to prevent node using arguments passed to the process. * \param[in] use_intra_process_comms True to use the optimized intra-process communication @@ -100,6 +101,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const std::string & namespace_, rclcpp::Context::SharedPtr context, const std::vector & arguments, + const std::vector & initial_parameters, bool use_global_arguments = true, bool use_intra_process_comms = false, bool start_parameter_services = true); diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index fe1a31c61d..cc86d5f03a 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -51,6 +51,7 @@ LifecycleNode::LifecycleNode( namespace_, rclcpp::contexts::default_context::get_global_default_context(), {}, + {}, true, use_intra_process_comms, true) @@ -61,6 +62,7 @@ LifecycleNode::LifecycleNode( const std::string & namespace_, rclcpp::Context::SharedPtr context, const std::vector & arguments, + const std::vector & initial_parameters, bool use_global_arguments, bool use_intra_process_comms, bool start_parameter_services) @@ -75,6 +77,7 @@ LifecycleNode::LifecycleNode( node_base_, node_topics_, node_services_, + initial_parameters, use_intra_process_comms, start_parameter_services )),