Skip to content

Commit

Permalink
Initialize params from yaml files
Browse files Browse the repository at this point in the history
  • Loading branch information
sloretz committed Jun 1, 2018
1 parent 97a5e3f commit 2cb890e
Showing 1 changed file with 87 additions and 2 deletions.
89 changes: 87 additions & 2 deletions rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

#include "rclcpp/node_interfaces/node_parameters.hpp"

#include <rcl_yaml_param_parser/parser.h>

#include <map>
#include <memory>
#include <string>
Expand All @@ -22,6 +24,7 @@

#include "rcl_interfaces/srv/list_parameters.hpp"
#include "rclcpp/create_publisher.hpp"
#include "rclcpp/parameter_map.hpp"
#include "rcutils/logging_macros.h"
#include "rmw/qos_profiles.h"

Expand Down Expand Up @@ -52,10 +55,92 @@ NodeParameters::NodeParameters(
use_intra_process,
allocator);

// Get the node options
const rcl_node_t * node = node_base->get_rcl_node_handle();
if (NULL == node) {
throw std::runtime_error("Need valid node handle in NodeParameters");
}
const rcl_node_options_t * options = rcl_node_get_options(node);
if (NULL == 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);
}
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) rcl too 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 (NULL == yaml_params) {
throw std::runtime_error("Failed to initialize yaml params struct");
}
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);
auto iter = initial_map.find(combined_name);
if (initial_map.end() == iter) {
continue;
}

// Combind 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_values) {
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
// Set initial parameter values
if (!initial_values.empty()) {
rcl_interfaces::msg::SetParametersResult result = set_parameters_atomically(initial_values);
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");
}
Expand Down

0 comments on commit 2cb890e

Please sign in to comment.