Pass parameter values to node constructor

sloretz · sloretz · commit ec0500b2748f · 2018-06-01T12:21:34.000-07:00
diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt
@@ -205,6 +205,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
@@ -96,6 +96,7 @@ class Node : public std::enable_shared_from_this<Node>
     const std::string & namespace_,
     rclcpp::Context::SharedPtr context,
     const std::vector<std::string> & arguments,
+    const std::vector<Parameter> & initial_values,
     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
@@ -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<Parameter> & initial_values,
     bool use_intra_process,
     bool start_parameter_services);
 
diff --git 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<std::string> & arguments,
+  const std::vector<Parameter> & initial_values,
   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_values,
       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
@@ -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<Parameter> & initial_values,
   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
+  // Set initial parameter values
+  if (!initial_values.empty()) {
+    rcl_interfaces::msg::SetParametersResult result = set_parameters_atomically(initial_values);
+    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
@@ -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<std::string> arguments = {"__node:=local_arguments_test"};
+  const std::vector<rclcpp::Parameter> 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<std::string> arguments = {};
+  const std::vector<rclcpp::Parameter> 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
@@ -0,0 +1,65 @@
+// 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 <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <vector>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class TestNodeWithInitialValues : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, NULL);
+  }
+};
+
+TEST_F(TestNodeWithInitialValues, no_initial_values) {
+  auto context = rclcpp::contexts::default_context::get_global_default_context();
+  const std::vector<std::string> arguments = {};
+  const std::vector<rclcpp::Parameter> 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, many_initial_values) {
+  auto context = rclcpp::contexts::default_context::get_global_default_context();
+  const std::vector<std::string> arguments = {};
+  const std::vector<rclcpp::Parameter> initial_values = {
+    rclcpp::Parameter("foo", true),
+    rclcpp::Parameter("bar", "hello world"),
+    rclcpp::Parameter("baz", std::vector<double>{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<bool>());
+  EXPECT_STREQ("hello world", node->get_parameter("bar").get_value<std::string>().c_str());
+  std::vector<double> double_array = node->get_parameter("baz").get_value<std::vector<double>>();
+  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
@@ -100,6 +100,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
     const std::string & namespace_,
     rclcpp::Context::SharedPtr context,
     const std::vector<std::string> & arguments,
+    const std::vector<rclcpp::Parameter> & initial_values,
     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
@@ -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<std::string> & arguments,
+  const std::vector<rclcpp::Parameter> & initial_values,
   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_values,
       use_intra_process_comms,
       start_parameter_services
     )),
