diff --git a/hardware_interface/src/lexical_casts.cpp b/hardware_interface/src/lexical_casts.cpp index b33cf2cb52..6de4c69ae1 100644 --- a/hardware_interface/src/lexical_casts.cpp +++ b/hardware_interface/src/lexical_casts.cpp @@ -29,6 +29,39 @@ namespace hardware_interface { namespace impl { +template +std::optional parse_floating_point_with_from_chars(const std::string & s) +{ + const char * begin = s.data(); + const char * end = s.data() + s.size(); + + if (begin == end) + { + return std::nullopt; + } + + // std::from_chars for floating-point does not accept a leading '+' sign. + if (*begin == '+') + { + ++begin; + if (begin == end) + { + return std::nullopt; + } + } + + FloatingPointType result_value; + const auto parse_result = std::from_chars(begin, end, result_value); + if ( + parse_result.ec == std::errc() && parse_result.ptr == end && + std::isfinite(static_cast(result_value))) + { + return result_value; + } + + return std::nullopt; +} + std::optional stod(const std::string & s) { #if __cplusplus < 202002L @@ -45,13 +78,7 @@ std::optional stod(const std::string & s) return result; #else // Impl with std::from_chars - double result_value; - const auto parse_result = std::from_chars(s.data(), s.data() + s.size(), result_value); - if (parse_result.ec == std::errc()) - { - return result_value; - } - return std::nullopt; + return parse_floating_point_with_from_chars(s); #endif } @@ -71,13 +98,7 @@ std::optional stof(const std::string & s) return result; #else // Impl with std::from_chars - float result_value; - const auto parse_result = std::from_chars(s.data(), s.data() + s.size(), result_value); - if (parse_result.ec == std::errc()) - { - return result_value; - } - return std::nullopt; + return parse_floating_point_with_from_chars(s); #endif }