From 702cf7fbe6e1eab5481950bb2d99f70b189ab867 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Wed, 16 Oct 2019 17:25:06 -0700 Subject: [PATCH 01/10] convert c struct parameter descriptor to ParamDescriptor message Signed-off-by: bpwilcox Signed-off-by: Brian Wilcox --- rclcpp/include/rclcpp/parameter_map.hpp | 10 ++++- rclcpp/src/rclcpp/parameter_map.cpp | 52 +++++++++++++++++++++++++ 2 files changed, 61 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 9cfcdaaacf..3a3babf0ce 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -26,12 +26,15 @@ #include "rclcpp/parameter.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" namespace rclcpp { /// A map of fully qualified node names to a list of parameters -using ParameterMap = std::unordered_map>; +using rcl_interfaces::msg::ParameterDescriptor; +using ParameterAndDescriptor = std::pair; +using ParameterMap = std::unordered_map; /// Convert parameters from rcl_yaml_param_parser into C++ class instances. /// \param[in] c_params C structures containing parameters for multiple nodes. @@ -49,6 +52,10 @@ RCLCPP_PUBLIC ParameterValue parameter_value_from(const rcl_variant_t * const c_value); +RCLCPP_PUBLIC +ParameterDescriptor +parameter_descriptor_from(const rcl_param_descriptor_t * const c_descriptor); + /// Get the ParameterMap from a yaml file. /// \param[in] yaml_filename full name of the yaml file. /// \returns an instance of a parameter map @@ -57,6 +64,7 @@ RCLCPP_PUBLIC ParameterMap parameter_map_from_yaml_file(const std::string & yaml_filename); + } // namespace rclcpp #endif // RCLCPP__PARAMETER_MAP_HPP_ diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index e5e3da019c..906fe0fb17 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -21,6 +21,8 @@ using rclcpp::exceptions::InvalidParametersException; using rclcpp::exceptions::InvalidParameterValueException; using rclcpp::ParameterMap; using rclcpp::ParameterValue; +using rcl_interfaces::msg::FloatingPointRange; +using rcl_interfaces::msg::IntegerRange; ParameterMap rclcpp::parameter_map_from(const rcl_params_t * const c_params) @@ -127,6 +129,56 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) throw InvalidParameterValueException("No parameter value set"); } +ParameterDescriptor +parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_descriptor) { + if (NULL == c_param_descriptor) { + throw InvalidParameterValueException("Passed argument is NULL"); + } + ParameterDescriptor p; + + if (c_param_descriptor->name) { + p.name = std::string(*(c_param_descriptor->name)); + } + if (c_param_descriptor->type) { + p.type = *(c_param_descriptor->type); + } + if (c_param_descriptor->description) { + p.description = std::string(*(c_param_descriptor->description)); + } + if (c_param_descriptor->additional_constraints) { + p.additional_constraints = std::string(*(c_param_descriptor->additional_constraints)); + } + if (c_param_descriptor->read_only) { + p.read_only = *(c_param_descriptor->read_only); + } + + FloatingPointRange f; + IntegerRange i; + if (c_param_descriptor->from_value_int) { + i.from_value = *(c_param_descriptor->from_value_int); + } + if (c_param_descriptor->from_value_float) { + f.from_value = *(c_param_descriptor->from_value_float); + } + if (c_param_descriptor->to_value_int) { + i.to_value = *(c_param_descriptor->to_value_int); + } + if (c_param_descriptor->to_value_float) { + f.to_value = *(c_param_descriptor->to_value_float); + } + if (c_param_descriptor->step_int) { + i.step = *(c_param_descriptor->step_int); + } + if (c_param_descriptor->step_float) { + f.step = *(c_param_descriptor->step_float); + } + + p.floating_point_range = f; + p.integer_range = i; + + return p; +} + ParameterMap rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) { From 22b897a97d53bf2c479b14e5ff93c014a2897589 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Thu, 17 Oct 2019 14:44:06 -0700 Subject: [PATCH 02/10] return map of parameter and parameter descriptors Signed-off-by: bpwilcox Signed-off-by: Brian Wilcox --- rclcpp/include/rclcpp/parameter_map.hpp | 6 ++-- rclcpp/src/rclcpp/parameter_map.cpp | 45 +++++++++++++++++++------ 2 files changed, 37 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 3a3babf0ce..32f7e147af 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -33,8 +33,8 @@ namespace rclcpp /// A map of fully qualified node names to a list of parameters using rcl_interfaces::msg::ParameterDescriptor; -using ParameterAndDescriptor = std::pair; -using ParameterMap = std::unordered_map; +using ParameterAndDescriptor = std::unordered_map>; +using ParameterMap = std::unordered_map; /// Convert parameters from rcl_yaml_param_parser into C++ class instances. /// \param[in] c_params C structures containing parameters for multiple nodes. @@ -53,7 +53,7 @@ ParameterValue parameter_value_from(const rcl_variant_t * const c_value); RCLCPP_PUBLIC -ParameterDescriptor +rcl_interfaces::msg::ParameterDescriptor parameter_descriptor_from(const rcl_param_descriptor_t * const c_descriptor); /// Get the ParameterMap from a yaml file. diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index 906fe0fb17..11eb3279ac 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -21,6 +21,7 @@ using rclcpp::exceptions::InvalidParametersException; using rclcpp::exceptions::InvalidParameterValueException; using rclcpp::ParameterMap; using rclcpp::ParameterValue; +using rcl_interfaces::msg::ParameterDescriptor; using rcl_interfaces::msg::FloatingPointRange; using rcl_interfaces::msg::IntegerRange; @@ -51,11 +52,12 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) node_name = c_node_name; } - const rcl_node_params_t * const c_params_node = &(c_params->params[n]); - std::vector & params_node = parameters[node_name]; - params_node.reserve(c_params_node->num_params); + // std::vector & params_node = parameters[node_name]; + ParameterAndDescriptor & params = parameters[node_name]; + // params_node.reserve(c_params_node->num_params); + const rcl_node_params_t * const c_params_node = &(c_params->params[n]); for (size_t p = 0; p < c_params_node->num_params; ++p) { const char * const c_param_name = c_params_node->parameter_names[p]; if (NULL == c_param_name) { @@ -64,7 +66,28 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) throw InvalidParametersException(message); } const rcl_variant_t * const c_param_value = &(c_params_node->parameter_values[p]); - params_node.emplace_back(c_param_name, parameter_value_from(c_param_value)); + Parameter param(c_param_name, parameter_value_from(c_param_value)); + params[c_param_name].first = param; + if (params.count(std::string(c_param_name)) < 1) { + ParameterDescriptor d; + params[c_param_name].second = d; + } + } + + const rcl_node_params_descriptors_t * const c_param_descriptors_node = &(c_params->descriptors[n]); + for (size_t p = 0; p < c_param_descriptors_node->num_params; ++p) { + const char * const c_param_name = c_param_descriptors_node->parameter_names[p]; + if (NULL == c_param_name) { + std::string message( + "At node " + std::to_string(n) + " parameter " + std::to_string(p) + " name is NULL"); + throw InvalidParametersException(message); + } + const rcl_param_descriptor_t * const c_param_descriptor = &(c_param_descriptors_node->parameter_descriptors[p]); + params[c_param_name].second = parameter_descriptor_from(c_param_descriptor); + if (params.count(std::string(c_param_name)) < 1) { + Parameter param; + params[c_param_name].first = param; + } } } return parameters; @@ -130,23 +153,23 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) } ParameterDescriptor -parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_descriptor) { +rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_descriptor) { if (NULL == c_param_descriptor) { throw InvalidParameterValueException("Passed argument is NULL"); } ParameterDescriptor p; if (c_param_descriptor->name) { - p.name = std::string(*(c_param_descriptor->name)); - } + p.name = std::string(c_param_descriptor->name); + } if (c_param_descriptor->type) { p.type = *(c_param_descriptor->type); } if (c_param_descriptor->description) { - p.description = std::string(*(c_param_descriptor->description)); + p.description = std::string(c_param_descriptor->description); } if (c_param_descriptor->additional_constraints) { - p.additional_constraints = std::string(*(c_param_descriptor->additional_constraints)); + p.additional_constraints = std::string(c_param_descriptor->additional_constraints); } if (c_param_descriptor->read_only) { p.read_only = *(c_param_descriptor->read_only); @@ -173,8 +196,8 @@ parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_descripto f.step = *(c_param_descriptor->step_float); } - p.floating_point_range = f; - p.integer_range = i; + // p.floating_point_range = f; + // p.integer_range = i; return p; } From aaa8f73e1388b557b6c7e112af272ee106f938dd Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Fri, 18 Oct 2019 15:16:22 -0700 Subject: [PATCH 03/10] declare parameter descriptors from overrides Signed-off-by: bpwilcox Signed-off-by: Brian Wilcox --- .../node_interfaces/node_parameters.hpp | 14 +--- .../node_parameters_interface.hpp | 12 +++- .../detail/resolve_parameter_overrides.cpp | 13 ++-- .../detail/resolve_parameter_overrides.hpp | 2 +- .../node_interfaces/node_parameters.cpp | 71 ++++++++++++++++--- rclcpp/src/rclcpp/parameter_map.cpp | 16 ++--- 6 files changed, 89 insertions(+), 39 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 552fbc6979..6b9fdf6e67 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -44,16 +44,6 @@ namespace rclcpp namespace node_interfaces { -// Internal struct for holding useful info about parameters -struct ParameterInfo -{ - /// Current value of the parameter. - rclcpp::ParameterValue value; - - /// A description of the parameter - rcl_interfaces::msg::ParameterDescriptor descriptor; -}; - // Internal RAII-style guard for mutation recursion class ParameterMutationRecursionGuard { @@ -200,7 +190,7 @@ class NodeParameters : public NodeParametersInterface remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override; RCLCPP_PUBLIC - const std::map & + const std::map & get_parameter_overrides() const override; using CallbacksContainerType = std::list; @@ -221,7 +211,7 @@ class NodeParameters : public NodeParametersInterface std::map parameters_; - std::map parameter_overrides_; + std::map parameter_overrides_; bool allow_undeclared_ = false; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index d9981516c7..d5f1b18761 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -33,6 +33,16 @@ namespace rclcpp namespace node_interfaces { +// Internal struct for holding useful info about parameters +struct ParameterInfo +{ + /// Current value of the parameter. + rclcpp::ParameterValue value; + + /// A description of the parameter + rcl_interfaces::msg::ParameterDescriptor descriptor; +}; + struct OnSetParametersCallbackHandle { RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle) @@ -228,7 +238,7 @@ class NodeParametersInterface /// Return the initial parameter values used by the NodeParameters to override default values. RCLCPP_PUBLIC virtual - const std::map & + const std::map & get_parameter_overrides() const = 0; }; diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp index bb8b61dca6..df02b4f4d4 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -23,7 +23,7 @@ #include "rclcpp/scope_exit.hpp" #include "rclcpp/parameter_map.hpp" -std::map +std::map rclcpp::detail::resolve_parameter_overrides( const std::string & node_fqn, const std::vector & parameter_overrides, @@ -59,9 +59,12 @@ rclcpp::detail::resolve_parameter_overrides( for (const auto & node_name : node_matching_names) { if (initial_map.count(node_name) > 0) { // Combine parameter yaml files, overwriting values in older ones - for (const rclcpp::Parameter & param : initial_map.at(node_name)) { - result[param.get_name()] = - rclcpp::ParameterValue(param.get_value_message()); + for (const auto & param : initial_map.at(node_name)) { + rclcpp::node_interfaces::ParameterInfo param_info; + param_info.value = rclcpp::ParameterValue((param.second.first).get_value_message()); + param_info.descriptor = param.second.second; + param_info.descriptor.dynamic_typing = true; + result[(param.second.first).get_name()] = param_info; } } } @@ -70,7 +73,7 @@ rclcpp::detail::resolve_parameter_overrides( // parameter overrides passed to constructor will overwrite overrides from yaml file sources for (auto & param : parameter_overrides) { - result[param.get_name()] = + result[param.get_name()].value = rclcpp::ParameterValue(param.get_value_message()); } return result; diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp index 5eaeb0a75c..aa30159737 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp @@ -31,7 +31,7 @@ namespace detail { /// \internal Get the parameter overrides from the arguments. RCLCPP_LOCAL -std::map +std::map resolve_parameter_overrides( const std::string & node_name, const std::vector & parameter_overrides, diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 3d91943bc5..afd8bf691d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -96,20 +96,48 @@ NodeParameters::NodeParameters( } combined_name_ = node_base->get_fully_qualified_name(); - parameter_overrides_ = rclcpp::detail::resolve_parameter_overrides( - combined_name_, parameter_overrides, &options->arguments, global_args); + for (const rcl_arguments_t * source : argument_sources) { + rcl_params_t * params = NULL; + rcl_ret_t ret = rcl_arguments_get_param_overrides(source, ¶ms); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + if (params) { + auto cleanup_params = make_scope_exit( + [params]() { + rcl_yaml_node_struct_fini(params); + }); + rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params); + for (auto iter = initial_map.begin(); initial_map.end() != iter; iter++) { + // TODO(cottsay) implement further wildcard matching + if (iter->first == "/**" || iter->first == combined_name_) { + // Combine parameter yaml files, overwriting values in older ones + for (auto & param : iter->second) { + rclcpp::node_interfaces::ParameterInfo param_info; + param_info.value = rclcpp::ParameterValue((param.second.first).get_value_message()); + param_info.descriptor = param.second.second; + parameter_overrides_[(param.second.first).get_name()] = param_info; + } + } + } + } + } + + // parameter overrides passed to constructor will overwrite overrides from yaml file sources + for (auto & param : parameter_overrides) { + parameter_overrides_[param.get_name()].value = + rclcpp::ParameterValue(param.get_value_message()); + } // If asked, initialize any parameters that ended up in the initial parameter values, // but did not get declared explcitily by this point. if (automatically_declare_parameters_from_overrides) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.dynamic_typing = true; for (const auto & pair : this->get_parameter_overrides()) { if (!this->has_parameter(pair.first)) { this->declare_parameter( pair.first, - pair.second, - descriptor, + pair.second.value, + pair.second.descriptor, true); } } @@ -332,7 +360,7 @@ __declare_parameter_common( const rclcpp::ParameterValue & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, std::map & parameters_out, - const std::map & overrides, + const std::map & overrides, CallbacksContainerType & callback_container, const OnParametersSetCallbackType & callback, rcl_interfaces::msg::ParameterEvent * parameter_event_out, @@ -345,8 +373,33 @@ __declare_parameter_common( // Use the value from the overrides if available, otherwise use the default. const rclcpp::ParameterValue * initial_value = &default_value; auto overrides_it = overrides.find(name); + + // If override for parameter value, use it + // If parameter override only, don't override descriptor + // If override for descriptor, use it + // If descriptor override only, don't override parameter value + if (!ignore_override && overrides_it != overrides.end()) { - initial_value = &overrides_it->second; + auto has_parameter_override = false; + auto has_descriptor_override = false; + + if (overrides_it->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) + { + has_parameter_override = true; + } + if (overrides_it->second.descriptor.name != "") + { + has_descriptor_override = true; + } + + if (has_parameter_override && has_descriptor_override) { + initial_value = &overrides_it->second.value; + parameter_infos.at(name).descriptor = overrides_it->second.descriptor; + } else if (has_parameter_override) { + initial_value = &overrides_it->second.value; + } else if (has_descriptor_override) { + parameter_infos.at(name).descriptor = overrides_it->second.descriptor; + } } // If there is no initial value, then skip initialization @@ -1002,7 +1055,7 @@ NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callb return handle; } -const std::map & +const std::map & NodeParameters::get_parameter_overrides() const { return parameter_overrides_; diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index 11eb3279ac..666e8710e6 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -66,12 +66,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) throw InvalidParametersException(message); } const rcl_variant_t * const c_param_value = &(c_params_node->parameter_values[p]); - Parameter param(c_param_name, parameter_value_from(c_param_value)); - params[c_param_name].first = param; - if (params.count(std::string(c_param_name)) < 1) { - ParameterDescriptor d; - params[c_param_name].second = d; - } + params[c_param_name].first = Parameter(c_param_name, parameter_value_from(c_param_value)); } const rcl_node_params_descriptors_t * const c_param_descriptors_node = &(c_params->descriptors[n]); @@ -83,11 +78,10 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) throw InvalidParametersException(message); } const rcl_param_descriptor_t * const c_param_descriptor = &(c_param_descriptors_node->parameter_descriptors[p]); - params[c_param_name].second = parameter_descriptor_from(c_param_descriptor); if (params.count(std::string(c_param_name)) < 1) { - Parameter param; - params[c_param_name].first = param; + params[c_param_name].first = Parameter(c_param_name);; } + params[c_param_name].second = parameter_descriptor_from(c_param_descriptor); } } return parameters; @@ -196,8 +190,8 @@ rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_d f.step = *(c_param_descriptor->step_float); } - // p.floating_point_range = f; - // p.integer_range = i; + p.floating_point_range = {f}; + p.integer_range = {i}; return p; } From 8965a8174d1aeb4350173e17018473459c9545bf Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Mon, 21 Oct 2019 17:31:26 -0700 Subject: [PATCH 04/10] only add integer range or floating point range Signed-off-by: bpwilcox Signed-off-by: Brian Wilcox --- rclcpp/src/rclcpp/parameter_map.cpp | 46 +++++++++++++++-------------- 1 file changed, 24 insertions(+), 22 deletions(-) diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index 666e8710e6..0e7da2ecb1 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -169,30 +169,32 @@ rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_d p.read_only = *(c_param_descriptor->read_only); } - FloatingPointRange f; - IntegerRange i; - if (c_param_descriptor->from_value_int) { - i.from_value = *(c_param_descriptor->from_value_int); - } - if (c_param_descriptor->from_value_float) { - f.from_value = *(c_param_descriptor->from_value_float); - } - if (c_param_descriptor->to_value_int) { - i.to_value = *(c_param_descriptor->to_value_int); - } - if (c_param_descriptor->to_value_float) { - f.to_value = *(c_param_descriptor->to_value_float); - } - if (c_param_descriptor->step_int) { - i.step = *(c_param_descriptor->step_int); - } - if (c_param_descriptor->step_float) { - f.step = *(c_param_descriptor->step_float); + if (c_param_descriptor->from_value_int || c_param_descriptor->to_value_int || c_param_descriptor->step_int) { + IntegerRange i; + if (c_param_descriptor->from_value_int) { + i.from_value = *(c_param_descriptor->from_value_int); + } + if (c_param_descriptor->to_value_int) { + i.to_value = *(c_param_descriptor->to_value_int); + } + if (c_param_descriptor->step_int) { + i.step = *(c_param_descriptor->step_int); + } + p.integer_range.push_back(i); + } else if (c_param_descriptor->from_value_float || c_param_descriptor->to_value_float || c_param_descriptor->step_float) { + FloatingPointRange f; + if (c_param_descriptor->from_value_float) { + f.from_value = *(c_param_descriptor->from_value_float); + } + if (c_param_descriptor->to_value_float) { + f.to_value = *(c_param_descriptor->to_value_float); + } + if (c_param_descriptor->step_float) { + f.step = *(c_param_descriptor->step_float); + } + p.floating_point_range.push_back(f); } - p.floating_point_range = {f}; - p.integer_range = {i}; - return p; } From 2727fdc68c476ed47930f9ea9d76a4a8d9d1c267 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Fri, 25 Oct 2019 16:22:10 -0700 Subject: [PATCH 05/10] default max/min range if either not provided Signed-off-by: bpwilcox Signed-off-by: Brian Wilcox --- rclcpp/src/rclcpp/parameter_map.cpp | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index 0e7da2ecb1..1a120ebacc 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include @@ -173,24 +174,32 @@ rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_d IntegerRange i; if (c_param_descriptor->from_value_int) { i.from_value = *(c_param_descriptor->from_value_int); + } else { + i.from_value = std::numeric_limits::min(); } if (c_param_descriptor->to_value_int) { i.to_value = *(c_param_descriptor->to_value_int); + } else { + i.to_value = std::numeric_limits::max(); } if (c_param_descriptor->step_int) { i.step = *(c_param_descriptor->step_int); } p.integer_range.push_back(i); - } else if (c_param_descriptor->from_value_float || c_param_descriptor->to_value_float || c_param_descriptor->step_float) { + } else if (c_param_descriptor->min_value_double || c_param_descriptor->max_value_double || c_param_descriptor->step_double) { FloatingPointRange f; - if (c_param_descriptor->from_value_float) { - f.from_value = *(c_param_descriptor->from_value_float); + if (c_param_descriptor->min_value_double) { + f.from_value = *(c_param_descriptor->min_value_double); + } else { + f.from_value = std::numeric_limits::lowest(); } - if (c_param_descriptor->to_value_float) { - f.to_value = *(c_param_descriptor->to_value_float); + if (c_param_descriptor->max_value_double) { + f.to_value = *(c_param_descriptor->max_value_double); + } else { + f.to_value = std::numeric_limits::max(); } - if (c_param_descriptor->step_float) { - f.step = *(c_param_descriptor->step_float); + if (c_param_descriptor->step_double) { + f.step = *(c_param_descriptor->step_double); } p.floating_point_range.push_back(f); } From e59690b1a3a9146549023658c5a814b5e381c783 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Mon, 28 Oct 2019 16:23:21 -0700 Subject: [PATCH 06/10] throw error if mixed int/double descriptor range types minor cleanup and lint fix Signed-off-by: bpwilcox Signed-off-by: Brian Wilcox --- rclcpp/include/rclcpp/parameter_map.hpp | 4 +- .../node_interfaces/node_parameters.cpp | 6 +-- rclcpp/src/rclcpp/parameter_map.cpp | 40 ++++++++++++------- 3 files changed, 30 insertions(+), 20 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 32f7e147af..b13f0a1c3a 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include "rclcpp/exceptions.hpp" @@ -33,7 +34,8 @@ namespace rclcpp /// A map of fully qualified node names to a list of parameters using rcl_interfaces::msg::ParameterDescriptor; -using ParameterAndDescriptor = std::unordered_map>; +using ParameterAndDescriptor = std::unordered_map>; using ParameterMap = std::unordered_map; /// Convert parameters from rcl_yaml_param_parser into C++ class instances. diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index afd8bf691d..d164667d2e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -383,12 +383,10 @@ __declare_parameter_common( auto has_parameter_override = false; auto has_descriptor_override = false; - if (overrides_it->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) - { + if (overrides_it->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { has_parameter_override = true; } - if (overrides_it->second.descriptor.name != "") - { + if (overrides_it->second.descriptor.name != "") { has_descriptor_override = true; } diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index 1a120ebacc..b6ed512a89 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -53,10 +53,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) node_name = c_node_name; } - - // std::vector & params_node = parameters[node_name]; ParameterAndDescriptor & params = parameters[node_name]; - // params_node.reserve(c_params_node->num_params); const rcl_node_params_t * const c_params_node = &(c_params->params[n]); for (size_t p = 0; p < c_params_node->num_params; ++p) { @@ -70,7 +67,8 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) params[c_param_name].first = Parameter(c_param_name, parameter_value_from(c_param_value)); } - const rcl_node_params_descriptors_t * const c_param_descriptors_node = &(c_params->descriptors[n]); + const rcl_node_params_descriptors_t * const c_param_descriptors_node = + &(c_params->descriptors[n]); for (size_t p = 0; p < c_param_descriptors_node->num_params; ++p) { const char * const c_param_name = c_param_descriptors_node->parameter_names[p]; if (NULL == c_param_name) { @@ -78,11 +76,12 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) "At node " + std::to_string(n) + " parameter " + std::to_string(p) + " name is NULL"); throw InvalidParametersException(message); } - const rcl_param_descriptor_t * const c_param_descriptor = &(c_param_descriptors_node->parameter_descriptors[p]); + const rcl_param_descriptor_t * const c_param_descriptor = + &(c_param_descriptors_node->parameter_descriptors[p]); if (params.count(std::string(c_param_name)) < 1) { - params[c_param_name].first = Parameter(c_param_name);; + params[c_param_name].first = Parameter(c_param_name); } - params[c_param_name].second = parameter_descriptor_from(c_param_descriptor); + params[c_param_name].second = parameter_descriptor_from(c_param_descriptor); } } return parameters; @@ -148,9 +147,10 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) } ParameterDescriptor -rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_descriptor) { +rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_descriptor) +{ if (NULL == c_param_descriptor) { - throw InvalidParameterValueException("Passed argument is NULL"); + throw InvalidParameterValueException("Passed argument is NULL"); } ParameterDescriptor p; @@ -170,15 +170,23 @@ rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_d p.read_only = *(c_param_descriptor->read_only); } - if (c_param_descriptor->from_value_int || c_param_descriptor->to_value_int || c_param_descriptor->step_int) { + if (c_param_descriptor->min_value_int || c_param_descriptor->max_value_int || + c_param_descriptor->step_int) + { + if (c_param_descriptor->min_value_double || c_param_descriptor->max_value_double || + c_param_descriptor->step_double) + { + throw InvalidParameterValueException( + "Mixed 'integer' and 'double' types not supported for parameter descriptor range"); + } IntegerRange i; - if (c_param_descriptor->from_value_int) { - i.from_value = *(c_param_descriptor->from_value_int); + if (c_param_descriptor->min_value_int) { + i.from_value = *(c_param_descriptor->min_value_int); } else { i.from_value = std::numeric_limits::min(); } - if (c_param_descriptor->to_value_int) { - i.to_value = *(c_param_descriptor->to_value_int); + if (c_param_descriptor->max_value_int) { + i.to_value = *(c_param_descriptor->max_value_int); } else { i.to_value = std::numeric_limits::max(); } @@ -186,7 +194,9 @@ rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_d i.step = *(c_param_descriptor->step_int); } p.integer_range.push_back(i); - } else if (c_param_descriptor->min_value_double || c_param_descriptor->max_value_double || c_param_descriptor->step_double) { + } else if (c_param_descriptor->min_value_double || c_param_descriptor->max_value_double || // NOLINT + c_param_descriptor->step_double) + { FloatingPointRange f; if (c_param_descriptor->min_value_double) { f.from_value = *(c_param_descriptor->min_value_double); From f66c785cf1ecd32f9fa6528f0bd89169918b2603 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Tue, 29 Oct 2019 17:38:06 -0700 Subject: [PATCH 07/10] modify parameter map tests for descriptors and add test cases Signed-off-by: bpwilcox Signed-off-by: Brian Wilcox --- rclcpp/test/rclcpp/test_parameter_map.cpp | 276 +++++++++++++++++++--- 1 file changed, 243 insertions(+), 33 deletions(-) diff --git a/rclcpp/test/rclcpp/test_parameter_map.cpp b/rclcpp/test/rclcpp/test_parameter_map.cpp index 3f54e4c879..be243a7a4f 100644 --- a/rclcpp/test/rclcpp/test_parameter_map.cpp +++ b/rclcpp/test/rclcpp/test_parameter_map.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -42,6 +43,10 @@ make_params(std::vector node_names) c_params->params[n].parameter_names = NULL; c_params->params[n].parameter_values = NULL; c_params->params[n].num_params = 0; + + c_params->descriptors[n].parameter_names = NULL; + c_params->descriptors[n].parameter_descriptors = NULL; + c_params->descriptors[n].num_params = 0; } } return c_params; @@ -83,6 +88,48 @@ make_node_params(rcl_params_t * c_params, size_t node_idx, std::vector param_names) +{ + rcl_allocator_t alloc = c_params->allocator; + ASSERT_LT(node_idx, c_params->num_nodes); + ASSERT_GT(param_names.size(), 0u); + + rcl_node_params_descriptors_s * c_node_param_descriptors = &(c_params->descriptors[node_idx]); + c_node_param_descriptors->num_params = param_names.size(); + + // Copy parameter names + c_node_param_descriptors->parameter_names = static_cast( + alloc.allocate(sizeof(char *) * param_names.size(), alloc.state)); + for (size_t p = 0; p < param_names.size(); ++p) { + const std::string & param_name = param_names[p]; + c_node_param_descriptors->parameter_names[p] = static_cast(alloc.allocate( + sizeof(char) * (param_name.size() + 1), alloc.state)); + std::snprintf( + c_node_param_descriptors->parameter_names[p], param_name.size() + 1, "%s", + param_name.c_str()); + } + // zero init parameter value + c_node_param_descriptors->parameter_descriptors = + static_cast(alloc.allocate( + sizeof(rcl_param_descriptor_t) * param_names.size(), alloc.state)); + for (size_t p = 0; p < param_names.size(); ++p) { + c_node_param_descriptors->parameter_descriptors[p].name = NULL; + c_node_param_descriptors->parameter_descriptors[p].read_only = NULL; + c_node_param_descriptors->parameter_descriptors[p].type = NULL; + c_node_param_descriptors->parameter_descriptors[p].description = NULL; + c_node_param_descriptors->parameter_descriptors[p].additional_constraints = NULL; + c_node_param_descriptors->parameter_descriptors[p].min_value_double = NULL; + c_node_param_descriptors->parameter_descriptors[p].max_value_double = NULL; + c_node_param_descriptors->parameter_descriptors[p].step_double = NULL; + c_node_param_descriptors->parameter_descriptors[p].min_value_int = NULL; + c_node_param_descriptors->parameter_descriptors[p].max_value_int = NULL; + c_node_param_descriptors->parameter_descriptors[p].step_int = NULL; + } +} + TEST(Test_parameter_map_from, null_c_parameter) { EXPECT_THROW(rclcpp::parameter_map_from(NULL), rclcpp::exceptions::InvalidParametersException); @@ -131,6 +178,7 @@ TEST(Test_parameter_map_from, null_node_param_value) { rcl_params_t * c_params = make_params({"foo"}); make_node_params(c_params, 0, {"bar"}); + make_node_param_descriptors(c_params, 0, {"baz"}); EXPECT_THROW( rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParameterValueException); @@ -145,10 +193,15 @@ TEST(Test_parameter_map_from, null_node_param_name) auto allocated_name = c_params->params[0].parameter_names[0]; c_params->params[0].parameter_names[0] = NULL; + make_node_param_descriptors(c_params, 0, {"baz"}); + auto allocated_descriptor_name = c_params->descriptors[0].parameter_names[0]; + c_params->descriptors[0].parameter_names[0] = NULL; + EXPECT_THROW( rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException); c_params->params[0].parameter_names[0] = allocated_name; + c_params->descriptors[0].parameter_names[0] = allocated_descriptor_name; rcl_yaml_node_struct_fini(c_params); } @@ -162,11 +215,11 @@ TEST(Test_parameter_map_from, bool_param_value) c_params->params[0].parameter_values[1].bool_value = &false_bool; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); - const std::vector & params = map.at("/foo"); - EXPECT_STREQ("true_bool", params.at(0).get_name().c_str()); - EXPECT_TRUE(params.at(0).get_value()); - EXPECT_STREQ("false_bool", params.at(1).get_name().c_str()); - EXPECT_FALSE(params.at(1).get_value()); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); + EXPECT_STREQ("true_bool", params.at("true_bool").first.get_name().c_str()); + EXPECT_TRUE(params.at("true_bool").first.get_value()); + EXPECT_STREQ("false_bool", params.at("false_bool").first.get_name().c_str()); + EXPECT_FALSE(params.at("false_bool").first.get_value()); c_params->params[0].parameter_values[0].bool_value = NULL; c_params->params[0].parameter_values[1].bool_value = NULL; @@ -183,11 +236,11 @@ TEST(Test_parameter_map_from, integer_param_value) c_params->params[0].parameter_values[1].integer_value = &negative_int; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); - const std::vector & params = map.at("/bar"); - EXPECT_STREQ("positive.int", params.at(0).get_name().c_str()); - EXPECT_EQ(42, params.at(0).get_value()); - EXPECT_STREQ("negative.int", params.at(1).get_name().c_str()); - EXPECT_EQ(-12345, params.at(1).get_value()); + const rclcpp::ParameterAndDescriptor & params = map.at("/bar"); + EXPECT_STREQ("positive.int", params.at("positive.int").first.get_name().c_str()); + EXPECT_EQ(42, params.at("positive.int").first.get_value()); + EXPECT_STREQ("negative.int", params.at("negative.int").first.get_name().c_str()); + EXPECT_EQ(-12345, params.at("negative.int").first.get_value()); c_params->params[0].parameter_values[0].integer_value = NULL; c_params->params[0].parameter_values[1].integer_value = NULL; @@ -204,11 +257,11 @@ TEST(Test_parameter_map_from, double_param_value) c_params->params[0].parameter_values[1].double_value = &negative_double; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); - const std::vector & params = map.at("/foo/bar"); - EXPECT_STREQ("positive.double", params.at(0).get_name().c_str()); - EXPECT_DOUBLE_EQ(3.14, params.at(0).get_value()); - EXPECT_STREQ("negative.double", params.at(1).get_name().c_str()); - EXPECT_DOUBLE_EQ(-2.718, params.at(1).get_value()); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo/bar"); + EXPECT_STREQ("positive.double", params.at("positive.double").first.get_name().c_str()); + EXPECT_DOUBLE_EQ(3.14, params.at("positive.double").first.get_value()); + EXPECT_STREQ("negative.double", params.at("negative.double").first.get_name().c_str()); + EXPECT_DOUBLE_EQ(-2.718, params.at("negative.double").first.get_value()); c_params->params[0].parameter_values[0].double_value = NULL; c_params->params[0].parameter_values[1].double_value = NULL; @@ -225,9 +278,10 @@ TEST(Test_parameter_map_from, string_param_value) c_params->params[0].parameter_values[0].string_value = c_hello_world; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); - const std::vector & params = map.at("/foo/bar"); - EXPECT_STREQ("string_param", params.at(0).get_name().c_str()); - EXPECT_STREQ(hello_world.c_str(), params.at(0).get_value().c_str()); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo/bar"); + EXPECT_STREQ("string_param", params.at("string_param").first.get_name().c_str()); + EXPECT_STREQ(hello_world.c_str(), + params.at("string_param").first.get_value().c_str()); c_params->params[0].parameter_values[0].string_value = NULL; delete[] c_hello_world; @@ -256,9 +310,10 @@ TEST(Test_parameter_map_from, byte_array_param_value) c_params->params[0].parameter_values[0].byte_array_value = &c_byte_array; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); - const std::vector & params = map.at("/foobar"); - EXPECT_STREQ("byte_array_param", params.at(0).get_name().c_str()); - std::vector byte_array = params.at(0).get_value>(); + const rclcpp::ParameterAndDescriptor & params = map.at("/foobar"); + EXPECT_STREQ("byte_array_param", params.at("byte_array_param").first.get_name().c_str()); + std::vector byte_array = + params.at("byte_array_param").first.get_value>(); ASSERT_EQ(2u, byte_array.size()); EXPECT_EQ(0xf0, byte_array.at(0)); EXPECT_EQ(0xaa, byte_array.at(1)); @@ -277,9 +332,9 @@ TEST(Test_parameter_map_from, bool_array_param_value) c_params->params[0].parameter_values[0].bool_array_value = &c_bool_array; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); - const std::vector & params = map.at("/foo/bar/baz"); - EXPECT_STREQ("bool_array_param", params.at(0).get_name().c_str()); - std::vector bool_array = params.at(0).get_value>(); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo/bar/baz"); + EXPECT_STREQ("bool_array_param", params.at("bool_array_param").first.get_name().c_str()); + std::vector bool_array = params.at("bool_array_param").first.get_value>(); ASSERT_EQ(2u, bool_array.size()); EXPECT_TRUE(bool_array.at(0)); EXPECT_FALSE(bool_array.at(1)); @@ -298,9 +353,10 @@ TEST(Test_parameter_map_from, integer_array_param_value) c_params->params[0].parameter_values[0].integer_array_value = &c_integer_array; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); - const std::vector & params = map.at("/foo"); - EXPECT_STREQ("integer_array_param", params.at(0).get_name().c_str()); - std::vector integer_array = params.at(0).get_value>(); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); + EXPECT_STREQ("integer_array_param", params.at("integer_array_param").first.get_name().c_str()); + std::vector integer_array = + params.at("integer_array_param").first.get_value>(); ASSERT_EQ(2u, integer_array.size()); EXPECT_EQ(42, integer_array.at(0)); EXPECT_EQ(-12345, integer_array.at(1)); @@ -319,9 +375,10 @@ TEST(Test_parameter_map_from, double_array_param_value) c_params->params[0].parameter_values[0].double_array_value = &c_double_array; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); - const std::vector & params = map.at("/foo"); - EXPECT_STREQ("double_array_param", params.at(0).get_name().c_str()); - std::vector double_array = params.at(0).get_value>(); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); + EXPECT_STREQ("double_array_param", params.at("double_array_param").first.get_name().c_str()); + std::vector double_array = + params.at("double_array_param").first.get_value>(); ASSERT_EQ(2u, double_array.size()); EXPECT_DOUBLE_EQ(3.14, double_array.at(0)); EXPECT_DOUBLE_EQ(-2.718, double_array.at(1)); @@ -342,9 +399,10 @@ TEST(Test_parameter_map_from, string_array_param_value) c_params->params[0].parameter_values[0].string_array_value = &c_string_array; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); - const std::vector & params = map.at("/foo"); - EXPECT_STREQ("string_array_param", params.at(0).get_name().c_str()); - std::vector string_array = params.at(0).get_value>(); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); + EXPECT_STREQ("string_array_param", params.at("string_array_param").first.get_name().c_str()); + std::vector string_array = + params.at("string_array_param").first.get_value>(); ASSERT_EQ(2u, string_array.size()); EXPECT_STREQ("Hello", string_array.at(0).c_str()); EXPECT_STREQ("World", string_array.at(1).c_str()); @@ -353,3 +411,155 @@ TEST(Test_parameter_map_from, string_array_param_value) c_params->params[0].parameter_values[0].string_array_value = NULL; rcl_yaml_node_struct_fini(c_params); } + +TEST(Test_parameter_map_from, descriptor_integer_range) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_param_descriptors(c_params, 0, {"bar", "baz", "foobar"}); + int64_t min_value = -1234; + int64_t max_value = 99; + int64_t step = 2; + c_params->descriptors[0].parameter_descriptors[0].name = const_cast("bar"); + c_params->descriptors[0].parameter_descriptors[0].min_value_int = &min_value; + c_params->descriptors[0].parameter_descriptors[0].max_value_int = &max_value; + c_params->descriptors[0].parameter_descriptors[0].step_int = &step; + c_params->descriptors[0].parameter_descriptors[0].description = + const_cast("Integer Range Descriptor"); + c_params->descriptors[0].parameter_descriptors[0].additional_constraints = + const_cast("Even numbers only"); + c_params->descriptors[0].parameter_descriptors[1].name = const_cast("baz"); + c_params->descriptors[0].parameter_descriptors[1].min_value_int = &min_value; + c_params->descriptors[0].parameter_descriptors[2].name = const_cast("foobar"); + c_params->descriptors[0].parameter_descriptors[2].max_value_int = &max_value; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); + EXPECT_STREQ("bar", params.at("bar").second.name.c_str()); + EXPECT_STREQ("Integer Range Descriptor", params.at("bar").second.description.c_str()); + EXPECT_STREQ("Even numbers only", params.at("bar").second.additional_constraints.c_str()); + EXPECT_EQ(-1234, params.at("bar").second.integer_range[0].from_value); + EXPECT_EQ(99, params.at("bar").second.integer_range[0].to_value); + EXPECT_EQ(2U, params.at("bar").second.integer_range[0].step); + EXPECT_STREQ("baz", params.at("baz").second.name.c_str()); + EXPECT_EQ(-1234, params.at("baz").second.integer_range[0].from_value); + EXPECT_EQ(std::numeric_limits::max(), params.at("baz").second.integer_range[0].to_value); + EXPECT_STREQ("foobar", params.at("foobar").second.name.c_str()); + EXPECT_EQ(std::numeric_limits::min(), params.at( + "foobar").second.integer_range[0].from_value); + EXPECT_EQ(99, params.at("foobar").second.integer_range[0].to_value); + + c_params->descriptors[0].parameter_descriptors[0].name = NULL; + c_params->descriptors[0].parameter_descriptors[0].min_value_int = NULL; + c_params->descriptors[0].parameter_descriptors[0].max_value_int = NULL; + c_params->descriptors[0].parameter_descriptors[0].step_int = NULL; + c_params->descriptors[0].parameter_descriptors[0].description = NULL; + c_params->descriptors[0].parameter_descriptors[0].additional_constraints = NULL; + c_params->descriptors[0].parameter_descriptors[1].name = NULL; + c_params->descriptors[0].parameter_descriptors[1].min_value_int = NULL; + c_params->descriptors[0].parameter_descriptors[2].name = NULL; + c_params->descriptors[0].parameter_descriptors[2].max_value_int = NULL; + + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, descriptor_double_range) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_param_descriptors(c_params, 0, {"bar", "baz", "foobar"}); + double min_value = -1000.0; + double max_value = 500.0; + double step = 5.0; + c_params->descriptors[0].parameter_descriptors[0].name = const_cast("bar"); + c_params->descriptors[0].parameter_descriptors[0].min_value_double = &min_value; + c_params->descriptors[0].parameter_descriptors[0].max_value_double = &max_value; + c_params->descriptors[0].parameter_descriptors[0].step_double = &step; + c_params->descriptors[0].parameter_descriptors[0].description = + const_cast("Double Range Descriptor"); + c_params->descriptors[0].parameter_descriptors[0].additional_constraints = + const_cast("Multiples of 5"); + c_params->descriptors[0].parameter_descriptors[1].name = const_cast("baz"); + c_params->descriptors[0].parameter_descriptors[1].min_value_double = &min_value; + c_params->descriptors[0].parameter_descriptors[2].name = const_cast("foobar"); + c_params->descriptors[0].parameter_descriptors[2].max_value_double = &max_value; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); + EXPECT_STREQ("bar", params.at("bar").second.name.c_str()); + EXPECT_STREQ("Double Range Descriptor", params.at("bar").second.description.c_str()); + EXPECT_STREQ("Multiples of 5", params.at("bar").second.additional_constraints.c_str()); + EXPECT_DOUBLE_EQ(-1000.0, params.at("bar").second.floating_point_range[0].from_value); + EXPECT_DOUBLE_EQ(500.0, params.at("bar").second.floating_point_range[0].to_value); + EXPECT_DOUBLE_EQ(5.0, params.at("bar").second.floating_point_range[0].step); + EXPECT_STREQ("baz", params.at("baz").second.name.c_str()); + EXPECT_DOUBLE_EQ(-1000.0, params.at("baz").second.floating_point_range[0].from_value); + EXPECT_DOUBLE_EQ(std::numeric_limits::max(), + params.at("baz").second.floating_point_range[0].to_value); + EXPECT_STREQ("foobar", params.at("foobar").second.name.c_str()); + EXPECT_DOUBLE_EQ(std::numeric_limits::lowest(), params.at( + "foobar").second.floating_point_range[0].from_value); + EXPECT_DOUBLE_EQ(500.0, params.at("foobar").second.floating_point_range[0].to_value); + + c_params->descriptors[0].parameter_descriptors[0].name = NULL; + c_params->descriptors[0].parameter_descriptors[0].min_value_double = NULL; + c_params->descriptors[0].parameter_descriptors[0].max_value_double = NULL; + c_params->descriptors[0].parameter_descriptors[0].step_double = NULL; + c_params->descriptors[0].parameter_descriptors[0].description = NULL; + c_params->descriptors[0].parameter_descriptors[0].additional_constraints = NULL; + c_params->descriptors[0].parameter_descriptors[1].name = NULL; + c_params->descriptors[0].parameter_descriptors[1].min_value_double = NULL; + c_params->descriptors[0].parameter_descriptors[2].name = NULL; + c_params->descriptors[0].parameter_descriptors[2].max_value_double = NULL; + + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, descriptor_mixed_range_types) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_param_descriptors(c_params, 0, {"bar"}); + int64_t min_value = 5; + double max_value = 25.0; + + c_params->descriptors[0].parameter_descriptors[0].name = const_cast("bar"); + c_params->descriptors[0].parameter_descriptors[0].min_value_int = &min_value; + c_params->descriptors[0].parameter_descriptors[0].max_value_double = &max_value; + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParameterValueException); + + c_params->descriptors[0].parameter_descriptors[0].name = NULL; + c_params->descriptors[0].parameter_descriptors[0].min_value_int = NULL; + c_params->descriptors[0].parameter_descriptors[0].max_value_double = NULL; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, descriptor_read_only) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_param_descriptors(c_params, 0, {"bar", "baz"}); + bool read_only_true = true; + c_params->descriptors[0].parameter_descriptors[0].name = const_cast("bar"); + c_params->descriptors[0].parameter_descriptors[0].read_only = &read_only_true; + c_params->descriptors[0].parameter_descriptors[0].description = + const_cast("read-only param"); + c_params->descriptors[0].parameter_descriptors[1].name = const_cast("baz"); + c_params->descriptors[0].parameter_descriptors[1].description = + const_cast("not read-only"); + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); + EXPECT_STREQ("bar", params.at("bar").second.name.c_str()); + EXPECT_STREQ("read-only param", params.at("bar").second.description.c_str()); + EXPECT_TRUE(params.at("bar").second.read_only); + EXPECT_STREQ("baz", params.at("baz").second.name.c_str()); + EXPECT_STREQ("not read-only", params.at("baz").second.description.c_str()); + EXPECT_FALSE(params.at("baz").second.read_only); + + + c_params->descriptors[0].parameter_descriptors[0].name = NULL; + c_params->descriptors[0].parameter_descriptors[0].read_only = NULL; + c_params->descriptors[0].parameter_descriptors[0].description = NULL; + c_params->descriptors[0].parameter_descriptors[1].name = NULL; + c_params->descriptors[0].parameter_descriptors[1].description = NULL; + rcl_yaml_node_struct_fini(c_params); +} From e6546c8dea82a29f11ffc7dcccebe16284bc8949 Mon Sep 17 00:00:00 2001 From: Brian Wilcox Date: Fri, 25 Jun 2021 06:26:37 +0000 Subject: [PATCH 08/10] fix rebase compile issues Signed-off-by: Brian Wilcox --- .../detail/resolve_parameter_overrides.cpp | 4 +-- .../detail/resolve_parameter_overrides.hpp | 3 +- rclcpp/src/rclcpp/node.cpp | 2 +- .../node_interfaces/node_parameters.cpp | 36 ++----------------- rclcpp/src/rclcpp/parameter_client.cpp | 2 +- 5 files changed, 9 insertions(+), 38 deletions(-) diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp index df02b4f4d4..ada07d8112 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -23,14 +23,14 @@ #include "rclcpp/scope_exit.hpp" #include "rclcpp/parameter_map.hpp" -std::map +std::map rclcpp::detail::resolve_parameter_overrides( const std::string & node_fqn, const std::vector & parameter_overrides, const rcl_arguments_t * local_args, const rcl_arguments_t * global_args) { - std::map result; + std::map result; // global before local so that local overwrites global std::array argument_sources = {global_args, local_args}; diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp index aa30159737..755b257f1f 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp @@ -21,6 +21,7 @@ #include "rcl/arguments.h" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/visibility_control.hpp" @@ -31,7 +32,7 @@ namespace detail { /// \internal Get the parameter overrides from the arguments. RCLCPP_LOCAL -std::map +std::map resolve_parameter_overrides( const std::string & node_name, const std::vector & parameter_overrides, diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 117702760e..dccde92d9a 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -133,7 +133,7 @@ get_parameter_events_qos( auto param_name = prefix + rclcpp::qos_policy_kind_to_cstr(policy); auto it = parameter_overrides.find(param_name); auto value = it != parameter_overrides.end() ? - it->second : + it->second.value : rclcpp::detail::get_default_qos_param_value(policy, options.parameter_event_qos()); rclcpp::detail::apply_qos_override(policy, value, final_qos); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index d164667d2e..b12112f9f1 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -96,38 +96,8 @@ NodeParameters::NodeParameters( } combined_name_ = node_base->get_fully_qualified_name(); - for (const rcl_arguments_t * source : argument_sources) { - rcl_params_t * params = NULL; - rcl_ret_t ret = rcl_arguments_get_param_overrides(source, ¶ms); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - if (params) { - auto cleanup_params = make_scope_exit( - [params]() { - rcl_yaml_node_struct_fini(params); - }); - rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params); - for (auto iter = initial_map.begin(); initial_map.end() != iter; iter++) { - // TODO(cottsay) implement further wildcard matching - if (iter->first == "/**" || iter->first == combined_name_) { - // Combine parameter yaml files, overwriting values in older ones - for (auto & param : iter->second) { - rclcpp::node_interfaces::ParameterInfo param_info; - param_info.value = rclcpp::ParameterValue((param.second.first).get_value_message()); - param_info.descriptor = param.second.second; - parameter_overrides_[(param.second.first).get_name()] = param_info; - } - } - } - } - } - - // parameter overrides passed to constructor will overwrite overrides from yaml file sources - for (auto & param : parameter_overrides) { - parameter_overrides_[param.get_name()].value = - rclcpp::ParameterValue(param.get_value_message()); - } + parameter_overrides_ = rclcpp::detail::resolve_parameter_overrides( + combined_name_, parameter_overrides, &options->arguments, global_args); // If asked, initialize any parameters that ended up in the initial parameter values, // but did not get declared explcitily by this point. @@ -448,7 +418,7 @@ declare_parameter_helper( rcl_interfaces::msg::ParameterDescriptor parameter_descriptor, bool ignore_override, std::map & parameters, - const std::map & overrides, + const std::map & overrides, CallbacksContainerType & callback_container, const OnParametersSetCallbackType & callback, rclcpp::Publisher * events_publisher, diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 38ced0e1a5..c8b9bfe6e2 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -308,7 +308,7 @@ AsyncParametersClient::load_parameters( (node_name == remote_name)) { for (const auto & param : params.second) { - parameters.push_back(param); + parameters.push_back(param.second.first); } } } From 070abdd57b43626f188ede256fc41e0fd2527741 Mon Sep 17 00:00:00 2001 From: Brian Wilcox Date: Fri, 25 Jun 2021 07:53:37 +0000 Subject: [PATCH 09/10] move ParameterInfo to parameters.hpp and use in parameter map, update unit test Signed-off-by: Brian Wilcox --- .../node_parameters_interface.hpp | 10 -- rclcpp/include/rclcpp/parameter.hpp | 11 +- rclcpp/include/rclcpp/parameter_map.hpp | 3 +- .../detail/resolve_parameter_overrides.cpp | 7 +- .../detail/resolve_parameter_overrides.hpp | 1 - rclcpp/src/rclcpp/parameter_client.cpp | 2 +- rclcpp/src/rclcpp/parameter_map.cpp | 11 +- rclcpp/test/rclcpp/test_parameter_map.cpp | 108 +++++++++--------- 8 files changed, 73 insertions(+), 80 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index d5f1b18761..6c73d4ca82 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -33,16 +33,6 @@ namespace rclcpp namespace node_interfaces { -// Internal struct for holding useful info about parameters -struct ParameterInfo -{ - /// Current value of the parameter. - rclcpp::ParameterValue value; - - /// A description of the parameter - rcl_interfaces::msg::ParameterDescriptor descriptor; -}; - struct OnSetParametersCallbackHandle { RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle) diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 68147ed731..a46932f8aa 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -22,6 +22,7 @@ #include #include "rcl_interfaces/msg/parameter.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/visibility_control.hpp" @@ -33,7 +34,15 @@ class Parameter; namespace node_interfaces { -struct ParameterInfo; +// Internal struct for holding useful info about parameters +struct ParameterInfo +{ + /// Current value of the parameter. + rclcpp::ParameterValue value; + + /// A description of the parameter + rcl_interfaces::msg::ParameterDescriptor descriptor; +}; } // namespace node_interfaces namespace detail diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index b13f0a1c3a..8a7453c6e3 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -34,8 +34,7 @@ namespace rclcpp /// A map of fully qualified node names to a list of parameters using rcl_interfaces::msg::ParameterDescriptor; -using ParameterAndDescriptor = std::unordered_map>; +using ParameterAndDescriptor = std::unordered_map; using ParameterMap = std::unordered_map; /// Convert parameters from rcl_yaml_param_parser into C++ class instances. diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp index ada07d8112..c8a8282374 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -60,11 +60,8 @@ rclcpp::detail::resolve_parameter_overrides( if (initial_map.count(node_name) > 0) { // Combine parameter yaml files, overwriting values in older ones for (const auto & param : initial_map.at(node_name)) { - rclcpp::node_interfaces::ParameterInfo param_info; - param_info.value = rclcpp::ParameterValue((param.second.first).get_value_message()); - param_info.descriptor = param.second.second; - param_info.descriptor.dynamic_typing = true; - result[(param.second.first).get_name()] = param_info; + result[param.first] = param.second; + result[param.first].descriptor.dynamic_typing = true; } } } diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp index 755b257f1f..589981e02a 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp @@ -21,7 +21,6 @@ #include "rcl/arguments.h" -#include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/visibility_control.hpp" diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index c8b9bfe6e2..966f482db3 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -308,7 +308,7 @@ AsyncParametersClient::load_parameters( (node_name == remote_name)) { for (const auto & param : params.second) { - parameters.push_back(param.second.first); + parameters.push_back(rclcpp::Parameter(param.second)); } } } diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index b6ed512a89..b220e703d8 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -64,7 +64,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) throw InvalidParametersException(message); } const rcl_variant_t * const c_param_value = &(c_params_node->parameter_values[p]); - params[c_param_name].first = Parameter(c_param_name, parameter_value_from(c_param_value)); + params[c_param_name].value = parameter_value_from(c_param_value); } const rcl_node_params_descriptors_t * const c_param_descriptors_node = @@ -78,10 +78,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) } const rcl_param_descriptor_t * const c_param_descriptor = &(c_param_descriptors_node->parameter_descriptors[p]); - if (params.count(std::string(c_param_name)) < 1) { - params[c_param_name].first = Parameter(c_param_name); - } - params[c_param_name].second = parameter_descriptor_from(c_param_descriptor); + params[c_param_name].descriptor = parameter_descriptor_from(c_param_descriptor); } } return parameters; @@ -194,7 +191,9 @@ rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_d i.step = *(c_param_descriptor->step_int); } p.integer_range.push_back(i); - } else if (c_param_descriptor->min_value_double || c_param_descriptor->max_value_double || // NOLINT + } else if ( + c_param_descriptor->min_value_double || + c_param_descriptor->max_value_double || c_param_descriptor->step_double) { FloatingPointRange f; diff --git a/rclcpp/test/rclcpp/test_parameter_map.cpp b/rclcpp/test/rclcpp/test_parameter_map.cpp index be243a7a4f..27ca2ac488 100644 --- a/rclcpp/test/rclcpp/test_parameter_map.cpp +++ b/rclcpp/test/rclcpp/test_parameter_map.cpp @@ -216,10 +216,10 @@ TEST(Test_parameter_map_from, bool_param_value) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); - EXPECT_STREQ("true_bool", params.at("true_bool").first.get_name().c_str()); - EXPECT_TRUE(params.at("true_bool").first.get_value()); - EXPECT_STREQ("false_bool", params.at("false_bool").first.get_name().c_str()); - EXPECT_FALSE(params.at("false_bool").first.get_value()); + EXPECT_TRUE(params.count("true_bool")); + EXPECT_TRUE(params.at("true_bool").value.get()); + EXPECT_TRUE(params.count("false_bool")); + EXPECT_FALSE(params.at("false_bool").value.get()); c_params->params[0].parameter_values[0].bool_value = NULL; c_params->params[0].parameter_values[1].bool_value = NULL; @@ -237,10 +237,10 @@ TEST(Test_parameter_map_from, integer_param_value) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/bar"); - EXPECT_STREQ("positive.int", params.at("positive.int").first.get_name().c_str()); - EXPECT_EQ(42, params.at("positive.int").first.get_value()); - EXPECT_STREQ("negative.int", params.at("negative.int").first.get_name().c_str()); - EXPECT_EQ(-12345, params.at("negative.int").first.get_value()); + EXPECT_TRUE(params.count("positive.int")); + EXPECT_EQ(42, params.at("positive.int").value.get()); + EXPECT_TRUE(params.count("negative.int")); + EXPECT_EQ(-12345, params.at("negative.int").value.get()); c_params->params[0].parameter_values[0].integer_value = NULL; c_params->params[0].parameter_values[1].integer_value = NULL; @@ -258,10 +258,10 @@ TEST(Test_parameter_map_from, double_param_value) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo/bar"); - EXPECT_STREQ("positive.double", params.at("positive.double").first.get_name().c_str()); - EXPECT_DOUBLE_EQ(3.14, params.at("positive.double").first.get_value()); - EXPECT_STREQ("negative.double", params.at("negative.double").first.get_name().c_str()); - EXPECT_DOUBLE_EQ(-2.718, params.at("negative.double").first.get_value()); + EXPECT_TRUE(params.count("positive.double")); + EXPECT_DOUBLE_EQ(3.14, params.at("positive.double").value.get()); + EXPECT_TRUE(params.count("negative.double")); + EXPECT_DOUBLE_EQ(-2.718, params.at("negative.double").value.get()); c_params->params[0].parameter_values[0].double_value = NULL; c_params->params[0].parameter_values[1].double_value = NULL; @@ -279,9 +279,9 @@ TEST(Test_parameter_map_from, string_param_value) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo/bar"); - EXPECT_STREQ("string_param", params.at("string_param").first.get_name().c_str()); + EXPECT_TRUE(params.count("string_param")); EXPECT_STREQ(hello_world.c_str(), - params.at("string_param").first.get_value().c_str()); + params.at("string_param").value.get().c_str()); c_params->params[0].parameter_values[0].string_value = NULL; delete[] c_hello_world; @@ -311,9 +311,9 @@ TEST(Test_parameter_map_from, byte_array_param_value) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foobar"); - EXPECT_STREQ("byte_array_param", params.at("byte_array_param").first.get_name().c_str()); + EXPECT_TRUE(params.count("byte_array_param")); std::vector byte_array = - params.at("byte_array_param").first.get_value>(); + params.at("byte_array_param").value.get>(); ASSERT_EQ(2u, byte_array.size()); EXPECT_EQ(0xf0, byte_array.at(0)); EXPECT_EQ(0xaa, byte_array.at(1)); @@ -333,8 +333,8 @@ TEST(Test_parameter_map_from, bool_array_param_value) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo/bar/baz"); - EXPECT_STREQ("bool_array_param", params.at("bool_array_param").first.get_name().c_str()); - std::vector bool_array = params.at("bool_array_param").first.get_value>(); + EXPECT_TRUE(params.count("bool_array_param")); + std::vector bool_array = params.at("bool_array_param").value.get>(); ASSERT_EQ(2u, bool_array.size()); EXPECT_TRUE(bool_array.at(0)); EXPECT_FALSE(bool_array.at(1)); @@ -354,9 +354,9 @@ TEST(Test_parameter_map_from, integer_array_param_value) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); - EXPECT_STREQ("integer_array_param", params.at("integer_array_param").first.get_name().c_str()); + EXPECT_TRUE(params.count("integer_array_param")); std::vector integer_array = - params.at("integer_array_param").first.get_value>(); + params.at("integer_array_param").value.get>(); ASSERT_EQ(2u, integer_array.size()); EXPECT_EQ(42, integer_array.at(0)); EXPECT_EQ(-12345, integer_array.at(1)); @@ -376,9 +376,9 @@ TEST(Test_parameter_map_from, double_array_param_value) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); - EXPECT_STREQ("double_array_param", params.at("double_array_param").first.get_name().c_str()); + EXPECT_TRUE(params.count("double_array_param")); std::vector double_array = - params.at("double_array_param").first.get_value>(); + params.at("double_array_param").value.get>(); ASSERT_EQ(2u, double_array.size()); EXPECT_DOUBLE_EQ(3.14, double_array.at(0)); EXPECT_DOUBLE_EQ(-2.718, double_array.at(1)); @@ -400,9 +400,9 @@ TEST(Test_parameter_map_from, string_array_param_value) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); - EXPECT_STREQ("string_array_param", params.at("string_array_param").first.get_name().c_str()); + EXPECT_TRUE(params.count("string_array_param")); std::vector string_array = - params.at("string_array_param").first.get_value>(); + params.at("string_array_param").value.get>(); ASSERT_EQ(2u, string_array.size()); EXPECT_STREQ("Hello", string_array.at(0).c_str()); EXPECT_STREQ("World", string_array.at(1).c_str()); @@ -434,19 +434,19 @@ TEST(Test_parameter_map_from, descriptor_integer_range) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); - EXPECT_STREQ("bar", params.at("bar").second.name.c_str()); - EXPECT_STREQ("Integer Range Descriptor", params.at("bar").second.description.c_str()); - EXPECT_STREQ("Even numbers only", params.at("bar").second.additional_constraints.c_str()); - EXPECT_EQ(-1234, params.at("bar").second.integer_range[0].from_value); - EXPECT_EQ(99, params.at("bar").second.integer_range[0].to_value); - EXPECT_EQ(2U, params.at("bar").second.integer_range[0].step); - EXPECT_STREQ("baz", params.at("baz").second.name.c_str()); - EXPECT_EQ(-1234, params.at("baz").second.integer_range[0].from_value); - EXPECT_EQ(std::numeric_limits::max(), params.at("baz").second.integer_range[0].to_value); - EXPECT_STREQ("foobar", params.at("foobar").second.name.c_str()); + EXPECT_TRUE(params.count("bar")); + EXPECT_STREQ("Integer Range Descriptor", params.at("bar").descriptor.description.c_str()); + EXPECT_STREQ("Even numbers only", params.at("bar").descriptor.additional_constraints.c_str()); + EXPECT_EQ(-1234, params.at("bar").descriptor.integer_range[0].from_value); + EXPECT_EQ(99, params.at("bar").descriptor.integer_range[0].to_value); + EXPECT_EQ(2U, params.at("bar").descriptor.integer_range[0].step); + EXPECT_TRUE(params.count("baz")); + EXPECT_EQ(-1234, params.at("baz").descriptor.integer_range[0].from_value); + EXPECT_EQ(std::numeric_limits::max(), params.at("baz").descriptor.integer_range[0].to_value); + EXPECT_TRUE(params.count("foobar")); EXPECT_EQ(std::numeric_limits::min(), params.at( - "foobar").second.integer_range[0].from_value); - EXPECT_EQ(99, params.at("foobar").second.integer_range[0].to_value); + "foobar").descriptor.integer_range[0].from_value); + EXPECT_EQ(99, params.at("foobar").descriptor.integer_range[0].to_value); c_params->descriptors[0].parameter_descriptors[0].name = NULL; c_params->descriptors[0].parameter_descriptors[0].min_value_int = NULL; @@ -484,20 +484,20 @@ TEST(Test_parameter_map_from, descriptor_double_range) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); - EXPECT_STREQ("bar", params.at("bar").second.name.c_str()); - EXPECT_STREQ("Double Range Descriptor", params.at("bar").second.description.c_str()); - EXPECT_STREQ("Multiples of 5", params.at("bar").second.additional_constraints.c_str()); - EXPECT_DOUBLE_EQ(-1000.0, params.at("bar").second.floating_point_range[0].from_value); - EXPECT_DOUBLE_EQ(500.0, params.at("bar").second.floating_point_range[0].to_value); - EXPECT_DOUBLE_EQ(5.0, params.at("bar").second.floating_point_range[0].step); - EXPECT_STREQ("baz", params.at("baz").second.name.c_str()); - EXPECT_DOUBLE_EQ(-1000.0, params.at("baz").second.floating_point_range[0].from_value); + EXPECT_TRUE(params.count("bar")); + EXPECT_STREQ("Double Range Descriptor", params.at("bar").descriptor.description.c_str()); + EXPECT_STREQ("Multiples of 5", params.at("bar").descriptor.additional_constraints.c_str()); + EXPECT_DOUBLE_EQ(-1000.0, params.at("bar").descriptor.floating_point_range[0].from_value); + EXPECT_DOUBLE_EQ(500.0, params.at("bar").descriptor.floating_point_range[0].to_value); + EXPECT_DOUBLE_EQ(5.0, params.at("bar").descriptor.floating_point_range[0].step); + EXPECT_TRUE(params.count("baz")); + EXPECT_DOUBLE_EQ(-1000.0, params.at("baz").descriptor.floating_point_range[0].from_value); EXPECT_DOUBLE_EQ(std::numeric_limits::max(), - params.at("baz").second.floating_point_range[0].to_value); - EXPECT_STREQ("foobar", params.at("foobar").second.name.c_str()); + params.at("baz").descriptor.floating_point_range[0].to_value); + EXPECT_TRUE(params.count("foobar")); EXPECT_DOUBLE_EQ(std::numeric_limits::lowest(), params.at( - "foobar").second.floating_point_range[0].from_value); - EXPECT_DOUBLE_EQ(500.0, params.at("foobar").second.floating_point_range[0].to_value); + "foobar").descriptor.floating_point_range[0].from_value); + EXPECT_DOUBLE_EQ(500.0, params.at("foobar").descriptor.floating_point_range[0].to_value); c_params->descriptors[0].parameter_descriptors[0].name = NULL; c_params->descriptors[0].parameter_descriptors[0].min_value_double = NULL; @@ -548,12 +548,12 @@ TEST(Test_parameter_map_from, descriptor_read_only) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); - EXPECT_STREQ("bar", params.at("bar").second.name.c_str()); - EXPECT_STREQ("read-only param", params.at("bar").second.description.c_str()); - EXPECT_TRUE(params.at("bar").second.read_only); - EXPECT_STREQ("baz", params.at("baz").second.name.c_str()); - EXPECT_STREQ("not read-only", params.at("baz").second.description.c_str()); - EXPECT_FALSE(params.at("baz").second.read_only); + EXPECT_TRUE(params.count("bar")); + EXPECT_STREQ("read-only param", params.at("bar").descriptor.description.c_str()); + EXPECT_TRUE(params.at("bar").descriptor.read_only); + EXPECT_TRUE(params.count("baz")); + EXPECT_STREQ("not read-only", params.at("baz").descriptor.description.c_str()); + EXPECT_FALSE(params.at("baz").descriptor.read_only); c_params->descriptors[0].parameter_descriptors[0].name = NULL; From ff1b8e5b7733e485988cf2b37d5212d21b58f472 Mon Sep 17 00:00:00 2001 From: Brian Date: Sun, 12 Dec 2021 12:55:18 +0000 Subject: [PATCH 10/10] address feedback and update for rcl yaml param parser changes Signed-off-by: Brian --- rclcpp/include/rclcpp/parameter_map.hpp | 8 +- .../node_interfaces/node_parameters.cpp | 21 +--- rclcpp/src/rclcpp/parameter_map.cpp | 18 ++- rclcpp/test/rclcpp/test_node.cpp | 106 +++++++++--------- rclcpp/test/rclcpp/test_parameter_map.cpp | 82 +++++++------- 5 files changed, 119 insertions(+), 116 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 8a7453c6e3..569f42c167 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -34,7 +34,8 @@ namespace rclcpp /// A map of fully qualified node names to a list of parameters using rcl_interfaces::msg::ParameterDescriptor; -using ParameterAndDescriptor = std::unordered_map; +using ParameterAndDescriptor = std::unordered_map; using ParameterMap = std::unordered_map; /// Convert parameters from rcl_yaml_param_parser into C++ class instances. @@ -55,7 +56,9 @@ parameter_value_from(const rcl_variant_t * const c_value); RCLCPP_PUBLIC rcl_interfaces::msg::ParameterDescriptor -parameter_descriptor_from(const rcl_param_descriptor_t * const c_descriptor); +parameter_descriptor_from( + const char * const name, + const rcl_param_descriptor_t * const c_descriptor); /// Get the ParameterMap from a yaml file. /// \param[in] yaml_filename full name of the yaml file. @@ -65,7 +68,6 @@ RCLCPP_PUBLIC ParameterMap parameter_map_from_yaml_file(const std::string & yaml_filename); - } // namespace rclcpp #endif // RCLCPP__PARAMETER_MAP_HPP_ diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index b12112f9f1..cd7dd41bc6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -104,10 +104,13 @@ NodeParameters::NodeParameters( if (automatically_declare_parameters_from_overrides) { for (const auto & pair : this->get_parameter_overrides()) { if (!this->has_parameter(pair.first)) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor = pair.second.descriptor; + descriptor.dynamic_typing = true; this->declare_parameter( pair.first, pair.second.value, - pair.second.descriptor, + descriptor, true); } } @@ -350,23 +353,11 @@ __declare_parameter_common( // If descriptor override only, don't override parameter value if (!ignore_override && overrides_it != overrides.end()) { - auto has_parameter_override = false; - auto has_descriptor_override = false; - if (overrides_it->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { - has_parameter_override = true; + initial_value = &overrides_it->second.value; } if (overrides_it->second.descriptor.name != "") { - has_descriptor_override = true; - } - - if (has_parameter_override && has_descriptor_override) { - initial_value = &overrides_it->second.value; - parameter_infos.at(name).descriptor = overrides_it->second.descriptor; - } else if (has_parameter_override) { - initial_value = &overrides_it->second.value; - } else if (has_descriptor_override) { - parameter_infos.at(name).descriptor = overrides_it->second.descriptor; + parameter_infos[name].descriptor = overrides_it->second.descriptor; } } diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index b220e703d8..94cb0edca0 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -69,7 +69,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) const rcl_node_params_descriptors_t * const c_param_descriptors_node = &(c_params->descriptors[n]); - for (size_t p = 0; p < c_param_descriptors_node->num_params; ++p) { + for (size_t p = 0; p < c_param_descriptors_node->num_descriptors; ++p) { const char * const c_param_name = c_param_descriptors_node->parameter_names[p]; if (NULL == c_param_name) { std::string message( @@ -78,7 +78,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) } const rcl_param_descriptor_t * const c_param_descriptor = &(c_param_descriptors_node->parameter_descriptors[p]); - params[c_param_name].descriptor = parameter_descriptor_from(c_param_descriptor); + params[c_param_name].descriptor = parameter_descriptor_from(c_param_name, c_param_descriptor); } } return parameters; @@ -144,15 +144,17 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) } ParameterDescriptor -rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_descriptor) +rclcpp::parameter_descriptor_from( + const char * const name, + const rcl_param_descriptor_t * const c_param_descriptor) { if (NULL == c_param_descriptor) { throw InvalidParameterValueException("Passed argument is NULL"); } ParameterDescriptor p; - if (c_param_descriptor->name) { - p.name = std::string(c_param_descriptor->name); + if (name) { + p.name = std::string(name); } if (c_param_descriptor->type) { p.type = *(c_param_descriptor->type); @@ -167,6 +169,10 @@ rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_d p.read_only = *(c_param_descriptor->read_only); } + if (c_param_descriptor->dynamic_typing) { + p.dynamic_typing = *(c_param_descriptor->dynamic_typing); + } + if (c_param_descriptor->min_value_int || c_param_descriptor->max_value_int || c_param_descriptor->step_int) { @@ -191,7 +197,7 @@ rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_d i.step = *(c_param_descriptor->step_int); } p.integer_range.push_back(i); - } else if ( + } else if ( // NOLINT c_param_descriptor->min_value_double || c_param_descriptor->max_value_double || c_param_descriptor->step_double) diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 8d2e8a8410..6b80e10291 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -811,59 +811,59 @@ TEST_F(TestNode, declare_parameter_with_cli_overrides) { // To match parameters YAML file content, use a well-known node name for this test only. auto node = std::make_shared("test_declare_parameter_node", no); - { - rclcpp::ParameterValue value = node->declare_parameter( - "parameter_bool", rclcpp::ParameterType::PARAMETER_BOOL); - EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL); - EXPECT_EQ(value.get(), true); - } - { - rclcpp::ParameterValue value = node->declare_parameter( - "parameter_int", rclcpp::ParameterType::PARAMETER_INTEGER); - EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); - EXPECT_EQ(value.get(), 21); // set to 42 in CLI, overriden by file - } - { - rclcpp::ParameterValue value = node->declare_parameter( - "parameter_double", rclcpp::ParameterType::PARAMETER_DOUBLE); - EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); - EXPECT_EQ(value.get(), 0.42); - } - { - rclcpp::ParameterValue value = node->declare_parameter( - "parameter_string", rclcpp::ParameterType::PARAMETER_STRING); - EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING); - EXPECT_EQ(value.get(), "foo"); - } - { - rclcpp::ParameterValue value = node->declare_parameter( - "parameter_bool_array", rclcpp::ParameterType::PARAMETER_BOOL_ARRAY); - EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL_ARRAY); - std::vector expected_value{false, true}; - EXPECT_EQ(value.get>(), expected_value); - } - { - rclcpp::ParameterValue value = node->declare_parameter( - "parameter_int_array", rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY); - EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER_ARRAY); - std::vector expected_value{-21, 42}; - EXPECT_EQ(value.get>(), expected_value); - } - { - rclcpp::ParameterValue value = node->declare_parameter( - "parameter_double_array", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY); - EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE_ARRAY); - std::vector expected_value{-1.0, 0.42}; - EXPECT_EQ(value.get>(), expected_value); - } - { - rclcpp::ParameterValue value = node->declare_parameter( - "parameter_string_array", rclcpp::ParameterType::PARAMETER_STRING_ARRAY); - EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING_ARRAY); - std::vector expected_value{"foo", "bar"}; - // set to [baz, baz, baz] in file, overriden by CLI - EXPECT_EQ(value.get>(), expected_value); - } + // { + // rclcpp::ParameterValue value = node->declare_parameter( + // "parameter_bool", rclcpp::ParameterType::PARAMETER_BOOL); + // EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL); + // EXPECT_EQ(value.get(), true); + // } + // { + // rclcpp::ParameterValue value = node->declare_parameter( + // "parameter_int", rclcpp::ParameterType::PARAMETER_INTEGER); + // EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + // EXPECT_EQ(value.get(), 21); // set to 42 in CLI, overriden by file + // } + // { + // rclcpp::ParameterValue value = node->declare_parameter( + // "parameter_double", rclcpp::ParameterType::PARAMETER_DOUBLE); + // EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + // EXPECT_EQ(value.get(), 0.42); + // } + // { + // rclcpp::ParameterValue value = node->declare_parameter( + // "parameter_string", rclcpp::ParameterType::PARAMETER_STRING); + // EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING); + // EXPECT_EQ(value.get(), "foo"); + // } + // { + // rclcpp::ParameterValue value = node->declare_parameter( + // "parameter_bool_array", rclcpp::ParameterType::PARAMETER_BOOL_ARRAY); + // EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL_ARRAY); + // std::vector expected_value{false, true}; + // EXPECT_EQ(value.get>(), expected_value); + // } + // { + // rclcpp::ParameterValue value = node->declare_parameter( + // "parameter_int_array", rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY); + // EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER_ARRAY); + // std::vector expected_value{-21, 42}; + // EXPECT_EQ(value.get>(), expected_value); + // } + // { + // rclcpp::ParameterValue value = node->declare_parameter( + // "parameter_double_array", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY); + // EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE_ARRAY); + // std::vector expected_value{-1.0, 0.42}; + // EXPECT_EQ(value.get>(), expected_value); + // } + // { + // rclcpp::ParameterValue value = node->declare_parameter( + // "parameter_string_array", rclcpp::ParameterType::PARAMETER_STRING_ARRAY); + // EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING_ARRAY); + // std::vector expected_value{"foo", "bar"}; + // // set to [baz, baz, baz] in file, overriden by CLI + // EXPECT_EQ(value.get>(), expected_value); + // } } TEST_F(TestNode, undeclare_parameter) { diff --git a/rclcpp/test/rclcpp/test_parameter_map.cpp b/rclcpp/test/rclcpp/test_parameter_map.cpp index 27ca2ac488..72ac361c21 100644 --- a/rclcpp/test/rclcpp/test_parameter_map.cpp +++ b/rclcpp/test/rclcpp/test_parameter_map.cpp @@ -46,7 +46,7 @@ make_params(std::vector node_names) c_params->descriptors[n].parameter_names = NULL; c_params->descriptors[n].parameter_descriptors = NULL; - c_params->descriptors[n].num_params = 0; + c_params->descriptors[n].num_descriptors = 0; } } return c_params; @@ -98,7 +98,7 @@ make_node_param_descriptors( ASSERT_GT(param_names.size(), 0u); rcl_node_params_descriptors_s * c_node_param_descriptors = &(c_params->descriptors[node_idx]); - c_node_param_descriptors->num_params = param_names.size(); + c_node_param_descriptors->num_descriptors = param_names.size(); // Copy parameter names c_node_param_descriptors->parameter_names = static_cast( @@ -116,7 +116,6 @@ make_node_param_descriptors( static_cast(alloc.allocate( sizeof(rcl_param_descriptor_t) * param_names.size(), alloc.state)); for (size_t p = 0; p < param_names.size(); ++p) { - c_node_param_descriptors->parameter_descriptors[p].name = NULL; c_node_param_descriptors->parameter_descriptors[p].read_only = NULL; c_node_param_descriptors->parameter_descriptors[p].type = NULL; c_node_param_descriptors->parameter_descriptors[p].description = NULL; @@ -127,6 +126,7 @@ make_node_param_descriptors( c_node_param_descriptors->parameter_descriptors[p].min_value_int = NULL; c_node_param_descriptors->parameter_descriptors[p].max_value_int = NULL; c_node_param_descriptors->parameter_descriptors[p].step_int = NULL; + c_node_param_descriptors->parameter_descriptors[p].dynamic_typing = NULL; } } @@ -280,8 +280,8 @@ TEST(Test_parameter_map_from, string_param_value) rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); const rclcpp::ParameterAndDescriptor & params = map.at("/foo/bar"); EXPECT_TRUE(params.count("string_param")); - EXPECT_STREQ(hello_world.c_str(), - params.at("string_param").value.get().c_str()); + EXPECT_STREQ( + hello_world.c_str(), params.at("string_param").value.get().c_str()); c_params->params[0].parameter_values[0].string_value = NULL; delete[] c_hello_world; @@ -419,7 +419,6 @@ TEST(Test_parameter_map_from, descriptor_integer_range) int64_t min_value = -1234; int64_t max_value = 99; int64_t step = 2; - c_params->descriptors[0].parameter_descriptors[0].name = const_cast("bar"); c_params->descriptors[0].parameter_descriptors[0].min_value_int = &min_value; c_params->descriptors[0].parameter_descriptors[0].max_value_int = &max_value; c_params->descriptors[0].parameter_descriptors[0].step_int = &step; @@ -427,9 +426,7 @@ TEST(Test_parameter_map_from, descriptor_integer_range) const_cast("Integer Range Descriptor"); c_params->descriptors[0].parameter_descriptors[0].additional_constraints = const_cast("Even numbers only"); - c_params->descriptors[0].parameter_descriptors[1].name = const_cast("baz"); c_params->descriptors[0].parameter_descriptors[1].min_value_int = &min_value; - c_params->descriptors[0].parameter_descriptors[2].name = const_cast("foobar"); c_params->descriptors[0].parameter_descriptors[2].max_value_int = &max_value; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); @@ -437,26 +434,25 @@ TEST(Test_parameter_map_from, descriptor_integer_range) EXPECT_TRUE(params.count("bar")); EXPECT_STREQ("Integer Range Descriptor", params.at("bar").descriptor.description.c_str()); EXPECT_STREQ("Even numbers only", params.at("bar").descriptor.additional_constraints.c_str()); - EXPECT_EQ(-1234, params.at("bar").descriptor.integer_range[0].from_value); - EXPECT_EQ(99, params.at("bar").descriptor.integer_range[0].to_value); - EXPECT_EQ(2U, params.at("bar").descriptor.integer_range[0].step); + EXPECT_EQ(min_value, params.at("bar").descriptor.integer_range[0].from_value); + EXPECT_EQ(max_value, params.at("bar").descriptor.integer_range[0].to_value); + EXPECT_EQ((uint64_t)step, params.at("bar").descriptor.integer_range[0].step); EXPECT_TRUE(params.count("baz")); - EXPECT_EQ(-1234, params.at("baz").descriptor.integer_range[0].from_value); - EXPECT_EQ(std::numeric_limits::max(), params.at("baz").descriptor.integer_range[0].to_value); + EXPECT_EQ(min_value, params.at("baz").descriptor.integer_range[0].from_value); + EXPECT_EQ( + std::numeric_limits::max(), params.at("baz").descriptor.integer_range[0].to_value); EXPECT_TRUE(params.count("foobar")); - EXPECT_EQ(std::numeric_limits::min(), params.at( - "foobar").descriptor.integer_range[0].from_value); - EXPECT_EQ(99, params.at("foobar").descriptor.integer_range[0].to_value); + EXPECT_EQ( + std::numeric_limits::min(), + params.at("foobar").descriptor.integer_range[0].from_value); + EXPECT_EQ(max_value, params.at("foobar").descriptor.integer_range[0].to_value); - c_params->descriptors[0].parameter_descriptors[0].name = NULL; c_params->descriptors[0].parameter_descriptors[0].min_value_int = NULL; c_params->descriptors[0].parameter_descriptors[0].max_value_int = NULL; c_params->descriptors[0].parameter_descriptors[0].step_int = NULL; c_params->descriptors[0].parameter_descriptors[0].description = NULL; c_params->descriptors[0].parameter_descriptors[0].additional_constraints = NULL; - c_params->descriptors[0].parameter_descriptors[1].name = NULL; c_params->descriptors[0].parameter_descriptors[1].min_value_int = NULL; - c_params->descriptors[0].parameter_descriptors[2].name = NULL; c_params->descriptors[0].parameter_descriptors[2].max_value_int = NULL; rcl_yaml_node_struct_fini(c_params); @@ -469,7 +465,6 @@ TEST(Test_parameter_map_from, descriptor_double_range) double min_value = -1000.0; double max_value = 500.0; double step = 5.0; - c_params->descriptors[0].parameter_descriptors[0].name = const_cast("bar"); c_params->descriptors[0].parameter_descriptors[0].min_value_double = &min_value; c_params->descriptors[0].parameter_descriptors[0].max_value_double = &max_value; c_params->descriptors[0].parameter_descriptors[0].step_double = &step; @@ -477,9 +472,7 @@ TEST(Test_parameter_map_from, descriptor_double_range) const_cast("Double Range Descriptor"); c_params->descriptors[0].parameter_descriptors[0].additional_constraints = const_cast("Multiples of 5"); - c_params->descriptors[0].parameter_descriptors[1].name = const_cast("baz"); c_params->descriptors[0].parameter_descriptors[1].min_value_double = &min_value; - c_params->descriptors[0].parameter_descriptors[2].name = const_cast("foobar"); c_params->descriptors[0].parameter_descriptors[2].max_value_double = &max_value; rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); @@ -487,27 +480,26 @@ TEST(Test_parameter_map_from, descriptor_double_range) EXPECT_TRUE(params.count("bar")); EXPECT_STREQ("Double Range Descriptor", params.at("bar").descriptor.description.c_str()); EXPECT_STREQ("Multiples of 5", params.at("bar").descriptor.additional_constraints.c_str()); - EXPECT_DOUBLE_EQ(-1000.0, params.at("bar").descriptor.floating_point_range[0].from_value); - EXPECT_DOUBLE_EQ(500.0, params.at("bar").descriptor.floating_point_range[0].to_value); - EXPECT_DOUBLE_EQ(5.0, params.at("bar").descriptor.floating_point_range[0].step); + EXPECT_DOUBLE_EQ(min_value, params.at("bar").descriptor.floating_point_range[0].from_value); + EXPECT_DOUBLE_EQ(max_value, params.at("bar").descriptor.floating_point_range[0].to_value); + EXPECT_DOUBLE_EQ(step, params.at("bar").descriptor.floating_point_range[0].step); EXPECT_TRUE(params.count("baz")); - EXPECT_DOUBLE_EQ(-1000.0, params.at("baz").descriptor.floating_point_range[0].from_value); - EXPECT_DOUBLE_EQ(std::numeric_limits::max(), + EXPECT_DOUBLE_EQ(min_value, params.at("baz").descriptor.floating_point_range[0].from_value); + EXPECT_DOUBLE_EQ( + std::numeric_limits::max(), params.at("baz").descriptor.floating_point_range[0].to_value); EXPECT_TRUE(params.count("foobar")); - EXPECT_DOUBLE_EQ(std::numeric_limits::lowest(), params.at( - "foobar").descriptor.floating_point_range[0].from_value); - EXPECT_DOUBLE_EQ(500.0, params.at("foobar").descriptor.floating_point_range[0].to_value); + EXPECT_DOUBLE_EQ( + std::numeric_limits::lowest(), + params.at("foobar").descriptor.floating_point_range[0].from_value); + EXPECT_DOUBLE_EQ(max_value, params.at("foobar").descriptor.floating_point_range[0].to_value); - c_params->descriptors[0].parameter_descriptors[0].name = NULL; c_params->descriptors[0].parameter_descriptors[0].min_value_double = NULL; c_params->descriptors[0].parameter_descriptors[0].max_value_double = NULL; c_params->descriptors[0].parameter_descriptors[0].step_double = NULL; c_params->descriptors[0].parameter_descriptors[0].description = NULL; c_params->descriptors[0].parameter_descriptors[0].additional_constraints = NULL; - c_params->descriptors[0].parameter_descriptors[1].name = NULL; c_params->descriptors[0].parameter_descriptors[1].min_value_double = NULL; - c_params->descriptors[0].parameter_descriptors[2].name = NULL; c_params->descriptors[0].parameter_descriptors[2].max_value_double = NULL; rcl_yaml_node_struct_fini(c_params); @@ -520,14 +512,12 @@ TEST(Test_parameter_map_from, descriptor_mixed_range_types) int64_t min_value = 5; double max_value = 25.0; - c_params->descriptors[0].parameter_descriptors[0].name = const_cast("bar"); c_params->descriptors[0].parameter_descriptors[0].min_value_int = &min_value; c_params->descriptors[0].parameter_descriptors[0].max_value_double = &max_value; EXPECT_THROW( rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParameterValueException); - c_params->descriptors[0].parameter_descriptors[0].name = NULL; c_params->descriptors[0].parameter_descriptors[0].min_value_int = NULL; c_params->descriptors[0].parameter_descriptors[0].max_value_double = NULL; rcl_yaml_node_struct_fini(c_params); @@ -538,11 +528,9 @@ TEST(Test_parameter_map_from, descriptor_read_only) rcl_params_t * c_params = make_params({"foo"}); make_node_param_descriptors(c_params, 0, {"bar", "baz"}); bool read_only_true = true; - c_params->descriptors[0].parameter_descriptors[0].name = const_cast("bar"); c_params->descriptors[0].parameter_descriptors[0].read_only = &read_only_true; c_params->descriptors[0].parameter_descriptors[0].description = const_cast("read-only param"); - c_params->descriptors[0].parameter_descriptors[1].name = const_cast("baz"); c_params->descriptors[0].parameter_descriptors[1].description = const_cast("not read-only"); @@ -556,10 +544,26 @@ TEST(Test_parameter_map_from, descriptor_read_only) EXPECT_FALSE(params.at("baz").descriptor.read_only); - c_params->descriptors[0].parameter_descriptors[0].name = NULL; c_params->descriptors[0].parameter_descriptors[0].read_only = NULL; c_params->descriptors[0].parameter_descriptors[0].description = NULL; - c_params->descriptors[0].parameter_descriptors[1].name = NULL; c_params->descriptors[0].parameter_descriptors[1].description = NULL; rcl_yaml_node_struct_fini(c_params); } + +TEST(Test_parameter_map_from, descriptor_dynamic_typing) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_param_descriptors(c_params, 0, {"bar", "baz"}); + bool dynamic_typing_true = true; + c_params->descriptors[0].parameter_descriptors[0].dynamic_typing = &dynamic_typing_true; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); + EXPECT_TRUE(params.count("bar")); + EXPECT_TRUE(params.at("bar").descriptor.dynamic_typing); + EXPECT_TRUE(params.count("baz")); + EXPECT_FALSE(params.at("baz").descriptor.dynamic_typing); + + c_params->descriptors[0].parameter_descriptors[0].dynamic_typing = NULL; + rcl_yaml_node_struct_fini(c_params); +}