Fix C++20 behavior changes of std::from_chars

hardware_interface/src/lexical_casts.cpp

@@ -29,6 +29,39 @@ namespace hardware_interface
 {
 namespace impl
 {
+template <typename FloatingPointType>
+std::optional<FloatingPointType> 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<double>(result_value)))
+  {
+    return result_value;
+  }
+
+  return std::nullopt;
+}
+
 std::optional<double> stod(const std::string & s)
 {
 #if __cplusplus < 202002L
@@ -45,13 +78,7 @@ std::optional<double> 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<double>(s);
 #endif
 }
 
@@ -71,13 +98,7 @@ std::optional<float> 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<float>(s);
 #endif
 }