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..6c73d4ca82 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -228,7 +228,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/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 9cfcdaaacf..569f42c167 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -20,18 +20,23 @@ #include #include +#include #include #include "rclcpp/exceptions.hpp" #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::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. @@ -49,6 +54,12 @@ RCLCPP_PUBLIC ParameterValue parameter_value_from(const rcl_variant_t * const c_value); +RCLCPP_PUBLIC +rcl_interfaces::msg::ParameterDescriptor +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. /// \returns an instance of a parameter map diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp index bb8b61dca6..c8a8282374 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}; @@ -59,9 +59,9 @@ 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)) { + result[param.first] = param.second; + result[param.first].descriptor.dynamic_typing = true; } } } @@ -70,7 +70,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..589981e02a 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.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 3d91943bc5..cd7dd41bc6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -102,13 +102,14 @@ NodeParameters::NodeParameters( // 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)) { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor = pair.second.descriptor; + descriptor.dynamic_typing = true; this->declare_parameter( pair.first, - pair.second, + pair.second.value, descriptor, true); } @@ -332,7 +333,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 +346,19 @@ __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; + if (overrides_it->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { + initial_value = &overrides_it->second.value; + } + if (overrides_it->second.descriptor.name != "") { + parameter_infos[name].descriptor = overrides_it->second.descriptor; + } } // If there is no initial value, then skip initialization @@ -397,7 +409,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, @@ -1002,7 +1014,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_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 38ced0e1a5..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); + parameters.push_back(rclcpp::Parameter(param.second)); } } } diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index e5e3da019c..94cb0edca0 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 @@ -21,6 +22,9 @@ 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; ParameterMap rclcpp::parameter_map_from(const rcl_params_t * const c_params) @@ -49,11 +53,9 @@ 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); + ParameterAndDescriptor & params = parameters[node_name]; + 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) { @@ -62,7 +64,21 @@ 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)); + params[c_param_name].value = parameter_value_from(c_param_value); + } + + 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_descriptors; ++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].descriptor = parameter_descriptor_from(c_param_name, c_param_descriptor); } } return parameters; @@ -127,6 +143,85 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) throw InvalidParameterValueException("No parameter value set"); } +ParameterDescriptor +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 (name) { + p.name = std::string(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); + } + + 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) + { + 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->min_value_int) { + i.from_value = *(c_param_descriptor->min_value_int); + } else { + i.from_value = std::numeric_limits::min(); + } + if (c_param_descriptor->max_value_int) { + i.to_value = *(c_param_descriptor->max_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 ( // NOLINT + c_param_descriptor->min_value_double || + c_param_descriptor->max_value_double || + c_param_descriptor->step_double) + { + FloatingPointRange f; + 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->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_double) { + f.step = *(c_param_descriptor->step_double); + } + p.floating_point_range.push_back(f); + } + + return p; +} + ParameterMap rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) { 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 3f54e4c879..72ac361c21 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_descriptors = 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_descriptors = 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].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; + c_node_param_descriptors->parameter_descriptors[p].dynamic_typing = 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_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; @@ -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_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; @@ -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_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; @@ -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_TRUE(params.count("string_param")); + 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; @@ -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_TRUE(params.count("byte_array_param")); + std::vector byte_array = + 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)); @@ -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_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)); @@ -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_TRUE(params.count("integer_array_param")); + std::vector integer_array = + 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)); @@ -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_TRUE(params.count("double_array_param")); + std::vector double_array = + 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)); @@ -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_TRUE(params.count("string_array_param")); + std::vector string_array = + 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()); @@ -353,3 +411,159 @@ 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].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].min_value_int = &min_value; + 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_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(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(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(max_value, params.at("foobar").descriptor.integer_range[0].to_value); + + 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].min_value_int = 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].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].min_value_double = &min_value; + 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_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(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(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(max_value, params.at("foobar").descriptor.floating_point_range[0].to_value); + + 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].min_value_double = 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].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].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].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].description = + const_cast("not read-only"); + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const rclcpp::ParameterAndDescriptor & params = map.at("/foo"); + 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].read_only = NULL; + c_params->descriptors[0].parameter_descriptors[0].description = 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); +}