diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 52ac98cdb3..55a86e8a4e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -14,6 +14,8 @@ #include "rclcpp/node_interfaces/node_parameters.hpp" +#include + #include #include #include @@ -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 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, ¶m_files); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + auto cleanup_param_files = make_scope_exit( + [¶m_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 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 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"); }