Skip to content

Commit 391db23

Browse files
author
bpwilcox
committed
throw error if mixed int/double descriptor range types
minor cleanup and lint fix Signed-off-by: bpwilcox <[email protected]>
1 parent ec60c50 commit 391db23

File tree

3 files changed

+30
-20
lines changed

3 files changed

+30
-20
lines changed

rclcpp/include/rclcpp/parameter_map.hpp

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919

2020
#include <string>
2121
#include <unordered_map>
22+
#include <utility>
2223
#include <vector>
2324

2425
#include "rclcpp/exceptions.hpp"
@@ -32,7 +33,8 @@ namespace rclcpp
3233

3334
/// A map of fully qualified node names to a list of parameters
3435
using rcl_interfaces::msg::ParameterDescriptor;
35-
using ParameterAndDescriptor = std::unordered_map<std::string, std::pair<Parameter, ParameterDescriptor>>;
36+
using ParameterAndDescriptor = std::unordered_map<std::string, std::pair<Parameter,
37+
ParameterDescriptor>>;
3638
using ParameterMap = std::unordered_map<std::string, ParameterAndDescriptor>;
3739

3840
/// Convert parameters from rcl_yaml_param_parser into C++ class instances.

rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -348,12 +348,10 @@ __declare_parameter_common(
348348
auto has_parameter_override = false;
349349
auto has_descriptor_override = false;
350350

351-
if (overrides_it->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET)
352-
{
351+
if (overrides_it->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) {
353352
has_parameter_override = true;
354353
}
355-
if (overrides_it->second.descriptor.name != "")
356-
{
354+
if (overrides_it->second.descriptor.name != "") {
357355
has_descriptor_override = true;
358356
}
359357

rclcpp/src/rclcpp/parameter_map.cpp

Lines changed: 25 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -53,10 +53,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params)
5353
node_name = c_node_name;
5454
}
5555

56-
57-
// std::vector<Parameter> & params_node = parameters[node_name];
5856
ParameterAndDescriptor & params = parameters[node_name];
59-
// params_node.reserve(c_params_node->num_params);
6057

6158
const rcl_node_params_t * const c_params_node = &(c_params->params[n]);
6259
for (size_t p = 0; p < c_params_node->num_params; ++p) {
@@ -70,19 +67,21 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params)
7067
params[c_param_name].first = Parameter(c_param_name, parameter_value_from(c_param_value));
7168
}
7269

73-
const rcl_node_params_descriptors_t * const c_param_descriptors_node = &(c_params->descriptors[n]);
70+
const rcl_node_params_descriptors_t * const c_param_descriptors_node =
71+
&(c_params->descriptors[n]);
7472
for (size_t p = 0; p < c_param_descriptors_node->num_params; ++p) {
7573
const char * const c_param_name = c_param_descriptors_node->parameter_names[p];
7674
if (NULL == c_param_name) {
7775
std::string message(
7876
"At node " + std::to_string(n) + " parameter " + std::to_string(p) + " name is NULL");
7977
throw InvalidParametersException(message);
8078
}
81-
const rcl_param_descriptor_t * const c_param_descriptor = &(c_param_descriptors_node->parameter_descriptors[p]);
79+
const rcl_param_descriptor_t * const c_param_descriptor =
80+
&(c_param_descriptors_node->parameter_descriptors[p]);
8281
if (params.count(std::string(c_param_name)) < 1) {
83-
params[c_param_name].first = Parameter(c_param_name);;
82+
params[c_param_name].first = Parameter(c_param_name);
8483
}
85-
params[c_param_name].second = parameter_descriptor_from(c_param_descriptor);
84+
params[c_param_name].second = parameter_descriptor_from(c_param_descriptor);
8685
}
8786
}
8887
return parameters;
@@ -148,9 +147,10 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value)
148147
}
149148

150149
ParameterDescriptor
151-
rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_descriptor) {
150+
rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_descriptor)
151+
{
152152
if (NULL == c_param_descriptor) {
153-
throw InvalidParameterValueException("Passed argument is NULL");
153+
throw InvalidParameterValueException("Passed argument is NULL");
154154
}
155155
ParameterDescriptor p;
156156

@@ -170,23 +170,33 @@ rclcpp::parameter_descriptor_from(const rcl_param_descriptor_t * const c_param_d
170170
p.read_only = *(c_param_descriptor->read_only);
171171
}
172172

173-
if (c_param_descriptor->from_value_int || c_param_descriptor->to_value_int || c_param_descriptor->step_int) {
173+
if (c_param_descriptor->min_value_int || c_param_descriptor->max_value_int ||
174+
c_param_descriptor->step_int)
175+
{
176+
if (c_param_descriptor->min_value_double || c_param_descriptor->max_value_double ||
177+
c_param_descriptor->step_double)
178+
{
179+
throw InvalidParameterValueException(
180+
"Mixed 'integer' and 'double' types not supported for parameter descriptor range");
181+
}
174182
IntegerRange i;
175-
if (c_param_descriptor->from_value_int) {
176-
i.from_value = *(c_param_descriptor->from_value_int);
183+
if (c_param_descriptor->min_value_int) {
184+
i.from_value = *(c_param_descriptor->min_value_int);
177185
} else {
178186
i.from_value = std::numeric_limits<int64_t>::min();
179187
}
180-
if (c_param_descriptor->to_value_int) {
181-
i.to_value = *(c_param_descriptor->to_value_int);
188+
if (c_param_descriptor->max_value_int) {
189+
i.to_value = *(c_param_descriptor->max_value_int);
182190
} else {
183191
i.to_value = std::numeric_limits<int64_t>::max();
184192
}
185193
if (c_param_descriptor->step_int) {
186194
i.step = *(c_param_descriptor->step_int);
187195
}
188196
p.integer_range.push_back(i);
189-
} else if (c_param_descriptor->min_value_double || c_param_descriptor->max_value_double || c_param_descriptor->step_double) {
197+
} else if (c_param_descriptor->min_value_double || c_param_descriptor->max_value_double || // NOLINT
198+
c_param_descriptor->step_double)
199+
{
190200
FloatingPointRange f;
191201
if (c_param_descriptor->min_value_double) {
192202
f.from_value = *(c_param_descriptor->min_value_double);

0 commit comments

Comments
 (0)