diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c969acee5b..bf5821673a 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -26,6 +26,9 @@ #include +#include +#include +#include #include #include #include @@ -185,6 +188,103 @@ __lockless_has_parameter( return parameters.find(name) != parameters.end(); } +// see https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon +RCLCPP_LOCAL +bool +__are_doubles_equal(double x, double y, size_t ulp = 100) +{ + return std::abs(x - y) <= std::numeric_limits::epsilon() * std::abs(x + y) * ulp; +} + +RCLCPP_LOCAL +inline +void +format_reason(std::string & reason, const std::string & name, const char * range_type) +{ + std::ostringstream ss; + ss << "Parameter {" << name << "} doesn't comply with " << range_type << " range."; + reason = ss.str(); +} + +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__check_parameter_value_in_range( + const rcl_interfaces::msg::ParameterDescriptor & descriptor, + const rclcpp::ParameterValue & value) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (!descriptor.integer_range.empty() && value.get_type() == rclcpp::PARAMETER_INTEGER) { + int64_t v = value.get(); + auto integer_range = descriptor.integer_range.at(0); + if (v == integer_range.from_value || v == integer_range.to_value) { + return result; + } + if ((v < integer_range.from_value) || (v > integer_range.to_value)) { + result.successful = false; + format_reason(result.reason, descriptor.name, "integer"); + return result; + } + if (integer_range.step == 0) { + return result; + } + if (((v - integer_range.from_value) % integer_range.step) == 0) { + return result; + } + result.successful = false; + format_reason(result.reason, descriptor.name, "integer"); + return result; + } + + if (!descriptor.floating_point_range.empty() && value.get_type() == rclcpp::PARAMETER_DOUBLE) { + double v = value.get(); + auto fp_range = descriptor.floating_point_range.at(0); + if (__are_doubles_equal(v, fp_range.from_value) || + __are_doubles_equal(v, fp_range.to_value)) + { + return result; + } + if ((v < fp_range.from_value) || (v > fp_range.to_value)) { + result.successful = false; + format_reason(result.reason, descriptor.name, "floating point"); + return result; + } + if (fp_range.step == 0.0) { + return result; + } + double rounded_div = std::round((v - fp_range.from_value) / fp_range.step); + if (__are_doubles_equal(v, fp_range.from_value + rounded_div * fp_range.step)) { + return result; + } + result.successful = false; + format_reason(result.reason, descriptor.name, "floating point"); + return result; + } + return result; +} + +// Return true if parameter values comply with the descriptors in parameter_infos. +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__check_parameters( + std::map & parameter_infos, + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const rclcpp::Parameter & parameter : parameters) { + const rcl_interfaces::msg::ParameterDescriptor & descriptor = + parameter_infos[parameter.get_name()].descriptor; + result = __check_parameter_value_in_range( + descriptor, + parameter.get_parameter_value()); + if (!result.successful) { + break; + } + } + return result; +} + using OnParametersSetCallbackType = rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; @@ -201,7 +301,14 @@ __set_parameters_atomically_common( if (on_set_parameters_callback) { result = on_set_parameters_callback(parameters); } - + if (!result.successful) { + return result; + } + // Check if the value being set complies with the descriptor. + result = __check_parameters(parameter_infos, parameters); + if (!result.successful) { + return result; + } // If accepted, actually set the values. if (result.successful) { for (size_t i = 0; i < parameters.size(); ++i) { diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp index bb3ac351f4..fca096f282 100644 --- a/rclcpp/test/test_node.cpp +++ b/rclcpp/test/test_node.cpp @@ -695,6 +695,353 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { EXPECT_FALSE(node->has_parameter(name)); } + { + // setting a parameter with integer range descriptor + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 10; + integer_range.to_value = 18; + integer_range.step = 2; + node->declare_parameter(name, 10, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 10); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 14)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 14); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 18)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 15)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 20)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 8)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor, from_value > to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 20; + integer_range.to_value = 18; + integer_range.step = 1; + node->declare_parameter(name, 20, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 20); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 18)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 10)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 25)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor, from_value = to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 18; + integer_range.to_value = 18; + integer_range.step = 1; + node->declare_parameter(name, 18, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 18); + + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 17)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor, step > distance(from_value, to_value) + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 18; + integer_range.to_value = 25; + integer_range.step = 10; + node->declare_parameter(name, 18, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 18); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 25)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 17)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 26)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + } + { + // setting a parameter with integer range descriptor, distance not multiple of the step. + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 18; + integer_range.to_value = 28; + integer_range.step = 7; + node->declare_parameter(name, 18, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 18); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 28)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 28); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 25)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 17)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 32)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + } + { + // setting a parameter with integer range descriptor, step=0 + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 10; + integer_range.to_value = 18; + integer_range.step = 0; + node->declare_parameter(name, 10, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 10); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 15)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 15); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 18)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor and wrong default value will throw + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 10; + integer_range.to_value = 18; + integer_range.step = 2; + ASSERT_THROW( + node->declare_parameter(name, 42, descriptor), + rclcpp::exceptions::InvalidParameterValueException); + } + { + // setting a parameter with floating point range descriptor + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.2); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.3)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, negative step + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = -0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.2); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.3)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, from_value > to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 11.0; + floating_point_range.to_value = 10.0; + floating_point_range.step = 0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, from_value = to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 10.0; + floating_point_range.step = 0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0); + } + { + // setting a parameter with floating point range descriptor, step > distance + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 2.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 7.8)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, distance not multiple of the step. + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 0.7; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.7)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.7); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.7); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.3)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.7); + } + { + // setting a parameter with floating point range descriptor, step=0 + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 0.0; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.0001)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0001); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.5479051)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.5479051); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.001)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.999)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with a different type is still possible + // when having a descriptor specifying a type (type is a status, not a constraint). + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rclcpp::PARAMETER_INTEGER; + node->declare_parameter(name, 42, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 42); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, "asd")).successful); + EXPECT_TRUE(node->has_parameter(name)); + value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING); + EXPECT_EQ(value.get_value(), "asd"); + } } TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) {