Initialize params via yaml from command line (#488)

* Initialize params from yaml files

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

@@ -14,6 +14,8 @@
 
 #include "rclcpp/node_interfaces/node_parameters.hpp"
 
+#include <rcl_yaml_param_parser/parser.h>
+
 #include <map>
 #include <memory>
 #include <string>
@@ -22,6 +24,8 @@
 
 #include "rcl_interfaces/srv/list_parameters.hpp"
 #include "rclcpp/create_publisher.hpp"
+#include "rclcpp/parameter_map.hpp"
+#include "rclcpp/scope_exit.hpp"
 #include "rcutils/logging_macros.h"
 #include "rmw/qos_profiles.h"
 
@@ -52,10 +56,100 @@ NodeParameters::NodeParameters(
     use_intra_process,
     allocator);
 
+  // Get the node options
+  const rcl_node_t * node = node_base->get_rcl_node_handle();
+  if (nullptr == node) {
+    throw std::runtime_error("Need valid node handle in NodeParameters");
+  }
+  const rcl_node_options_t * options = rcl_node_get_options(node);
+  if (nullptr == options) {
+    throw std::runtime_error("Need valid node options NodeParameters");
+  }
+
+  // Get paths to yaml files containing initial parameter values
+  std::vector<std::string> yaml_paths;
+
+  auto get_yaml_paths = [&yaml_paths, &options](const rcl_arguments_t * args) {
+      int num_yaml_files = rcl_arguments_get_param_files_count(args);
+      if (num_yaml_files > 0) {
+        char ** param_files;
+        rcl_ret_t ret = rcl_arguments_get_param_files(args, options->allocator, &param_files);
+        if (RCL_RET_OK != ret) {
+          rclcpp::exceptions::throw_from_rcl_error(ret);
+        }
+        auto cleanup_param_files = make_scope_exit(
+          [&param_files, &num_yaml_files, &options]() {
+            for (int i = 0; i < num_yaml_files; ++i) {
+              options->allocator.deallocate(param_files[i], options->allocator.state);
+            }
+            options->allocator.deallocate(param_files, options->allocator.state);
+          });
+        for (int i = 0; i < num_yaml_files; ++i) {
+          yaml_paths.emplace_back(param_files[i]);
+        }
+      }
+    };
+
+  // global before local so that local overwrites global
+  if (options->use_global_arguments) {
+    get_yaml_paths(rcl_get_global_arguments());
+  }
+  get_yaml_paths(&(options->arguments));
+
+  // Get fully qualified node name post-remapping to use to find node's params in yaml files
+  const std::string node_name = node_base->get_name();
+  const std::string node_namespace = node_base->get_namespace();
+  if (0u == node_namespace.size() || 0u == node_name.size()) {
+    // Should never happen
+    throw std::runtime_error("Node name and namespace were not set");
+  }
+  std::string combined_name;
+  if ('/' == node_namespace.at(node_namespace.size() - 1)) {
+    combined_name = node_namespace + node_name;
+  } else {
+    combined_name = node_namespace + '/' + node_name;
+  }
+
+  std::map<std::string, rclcpp::Parameter> parameters;
+
+  // TODO(sloretz) use rcl to parse yaml when circular dependency is solved
+  for (const std::string & yaml_path : yaml_paths) {
+    rcl_params_t * yaml_params = rcl_yaml_node_struct_init(options->allocator);
+    if (nullptr == yaml_params) {
+      throw std::bad_alloc();
+    }
+    if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params)) {
+      throw std::runtime_error("Failed to parse parameters " + yaml_path);
+    }
+
+    rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(yaml_params);
+    rcl_yaml_node_struct_fini(yaml_params);
+    auto iter = initial_map.find(combined_name);
+    if (initial_map.end() == iter) {
+      continue;
+    }
+
+    // Combine parameter yaml files, overwriting values in older ones
+    for (auto & param : iter->second) {
+      parameters[param.get_name()] = param;
+    }
+  }
+
+  // initial values passed to constructor overwrite yaml file sources
+  for (auto & param : initial_parameters) {
+    parameters[param.get_name()] = param;
+  }
+
+  std::vector<rclcpp::Parameter> combined_values;
+  combined_values.reserve(parameters.size());
+  for (auto & kv : parameters) {
+    combined_values.emplace_back(kv.second);
+  }
+
   // 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 (!combined_values.empty()) {
+    rcl_interfaces::msg::SetParametersResult result = set_parameters_atomically(combined_values);
     if (!result.successful) {
       throw std::runtime_error("Failed to set initial parameters");
     }