From 945a70e69c8035e91e471080405677c41ae20cdb Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 1 Jun 2018 15:49:05 -0700 Subject: [PATCH 1/9] Initialize params from yaml files --- .../node_interfaces/node_parameters.cpp | 89 ++++++++++++++++++- 1 file changed, 87 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 52ac98cdb3..490261a4a8 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,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" @@ -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 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); + } + 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) 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_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"); } From f067f5f5b50807f5ea94ed7d1b68a77c1afa6f73 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 5 Jun 2018 16:02:10 -0700 Subject: [PATCH 2/9] Style fixes --- .../node_interfaces/node_parameters.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 490261a4a8..24b0c56919 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -68,19 +68,19 @@ NodeParameters::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); - } - for (int i = 0; i < num_yaml_files; ++i) { - yaml_paths.emplace_back(param_files[i]); + 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); + } + 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) { From 818b7adbddd0f1434d9570ba3ac845069ddc0597 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 6 Jun 2018 01:49:37 -0700 Subject: [PATCH 3/9] typo --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 24b0c56919..8b91163b91 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -120,7 +120,7 @@ NodeParameters::NodeParameters( continue; } - // Combind parameter yaml files, overwriting values in older ones + // Combine parameter yaml files, overwriting values in older ones for (auto & param : iter->second) { parameters[param.get_name()] = param; } From bfb497c95ccb283547a4fbbb3c704c12b87ae8e9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 6 Jun 2018 09:02:06 -0700 Subject: [PATCH 4/9] NULL -> nullptr --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 8b91163b91..4fecba053a 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -57,11 +57,11 @@ NodeParameters::NodeParameters( // Get the node options const rcl_node_t * node = node_base->get_rcl_node_handle(); - if (NULL == node) { + 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 (NULL == options) { + if (nullptr == options) { throw std::runtime_error("Need valid node options NodeParameters"); } @@ -107,7 +107,7 @@ NodeParameters::NodeParameters( // 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) { + if (nullptr == yaml_params) { throw std::runtime_error("Failed to initialize yaml params struct"); } if (!rcl_parse_yaml_file(yaml_path.c_str(), yaml_params)) { From 934fa7797654cda5f481ee20766c188087faec6d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 6 Jun 2018 09:13:38 -0700 Subject: [PATCH 5/9] Deallocate param_files --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 4fecba053a..75066e0628 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -78,7 +78,9 @@ NodeParameters::NodeParameters( } for (int i = 0; i < num_yaml_files; ++i) { yaml_paths.emplace_back(param_files[i]); + options->allocator.deallocate(param_files[i], options->allocator.state); } + options->allocator.deallocate(param_files, options->allocator.state); } }; From 25716b115dbed7407e03161d55f8b79dd49a4055 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 6 Jun 2018 09:14:28 -0700 Subject: [PATCH 6/9] rcl too -> use rcl to --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 75066e0628..0924eb9b95 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -106,7 +106,7 @@ NodeParameters::NodeParameters( std::map parameters; - // TODO(sloretz) rcl too parse yaml when circular dependency is solved + // 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) { From d765738e346fd5ad815fccfd26d6c73989be950e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 6 Jun 2018 09:18:39 -0700 Subject: [PATCH 7/9] Finalize c yaml struct --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 0924eb9b95..0ee92d7446 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -117,6 +117,7 @@ NodeParameters::NodeParameters( } 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; From ef427a7e3c1d8d8b1c7f04e01a4de318694618bb Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 6 Jun 2018 09:22:18 -0700 Subject: [PATCH 8/9] runtime_error -> bad_alloc --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 0ee92d7446..49b5caffaa 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -110,7 +110,7 @@ NodeParameters::NodeParameters( 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::runtime_error("Failed to initialize yaml params struct"); + 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); From b9a7e7472557cfc1f5afc8cd58b30410707f1f67 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 7 Jun 2018 15:48:54 -0700 Subject: [PATCH 9/9] Exception safe cleanup --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 49b5caffaa..55a86e8a4e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -25,6 +25,7 @@ #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" @@ -76,11 +77,16 @@ NodeParameters::NodeParameters( 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]); - options->allocator.deallocate(param_files[i], options->allocator.state); } - options->allocator.deallocate(param_files, options->allocator.state); } };