diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 922ce9e4d1..6dc5090a1f 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -201,12 +201,10 @@ __check_parameter_value_in_range( 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)) { + const int64_t v = value.get(); + const auto& integer_range = descriptor.integer_range.at(0); + const bool within_range = (v >= integer_range.from_value) && (v <= integer_range.to_value); + if (!within_range) { result.successful = false; result.reason = format_range_reason(descriptor.name, "integer"); return result; @@ -223,12 +221,10 @@ __check_parameter_value_in_range( } 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)) { + const double v = value.get(); + const auto& fp_range = descriptor.floating_point_range.at(0); + const bool within_range = (v >= fp_range.from_value) && (v <= fp_range.to_value); + if (!within_range) { result.successful = false; result.reason = format_range_reason(descriptor.name, "floating point"); return result; @@ -236,7 +232,7 @@ __check_parameter_value_in_range( if (fp_range.step == 0.0) { return result; } - double rounded_div = std::round((v - fp_range.from_value) / fp_range.step); + const 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; } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 2826a80d7a..365c451e92 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -200,6 +200,69 @@ TEST_F(TestNodeParameters, set_parameters) { EXPECT_TRUE(result[0].successful); } +TEST_F(TestNodeParameters, double_parameter_range) +{ + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true); + + rcl_interfaces::msg::ParameterDescriptor double_descriptor; + double_descriptor.name = "double_parameter"; + double_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + double_descriptor.read_only = false; + + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = -1.0; + range.to_value = 666.0; + double_descriptor.floating_point_range.push_back(range); + + const auto default_value = rclcpp::ParameterValue(0.0); + + const auto loaded_value = node->declare_parameter( + double_descriptor.name, + default_value, + double_descriptor + ); + EXPECT_EQ(loaded_value, default_value); + + { + const rclcpp::Parameter new_parameter = rclcpp::Parameter( + "double_parameter", + std::numeric_limits::infinity() + ); + const auto result = node->set_parameter(new_parameter); + EXPECT_FALSE(result.successful); + EXPECT_EQ( + "Parameter {double_parameter} doesn't comply with floating point range.", + result.reason + ); + } + + { + const rclcpp::Parameter new_parameter = rclcpp::Parameter( + "double_parameter", + std::numeric_limits::quiet_NaN() + ); + const auto result = node->set_parameter(new_parameter); + EXPECT_FALSE(result.successful); + EXPECT_EQ( + "Parameter {double_parameter} doesn't comply with floating point range.", + result.reason + ); + } + + { + const rclcpp::Parameter new_parameter = rclcpp::Parameter("double_parameter", range.to_value); + const auto result = node->set_parameter(new_parameter); + EXPECT_TRUE(result.successful); + } + + { + const rclcpp::Parameter new_parameter = rclcpp::Parameter("double_parameter", range.from_value); + const auto result = node->set_parameter(new_parameter); + EXPECT_TRUE(result.successful); + } +} + TEST_F(TestNodeParameters, add_remove_on_set_parameters_callback) { rcl_interfaces::msg::ParameterDescriptor bool_descriptor; bool_descriptor.name = "bool_parameter";