From 9c3a2781bd673bdc4ed8e7bd8342c2b702a861e2 Mon Sep 17 00:00:00 2001
From: bailaC <subhas.robotics@gmail.com>
Date: Fri, 12 Jan 2024 22:39:37 +0530
Subject: [PATCH] Adding backward compatibility for string-to-double conversion
 (#1284)

---------

Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>
Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
(cherry picked from commit 477c85da524ba3eb9f587a30e4747b5fdeb725ef)
---
 .../hardware_interface/lexical_casts.hpp      |  1 +
 hardware_interface/src/lexical_casts.cpp      | 28 +++++++++++++++++--
 2 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/hardware_interface/include/hardware_interface/lexical_casts.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp
index 1b9bad7018..846d9f757c 100644
--- a/hardware_interface/include/hardware_interface/lexical_casts.hpp
+++ b/hardware_interface/include/hardware_interface/lexical_casts.hpp
@@ -16,6 +16,7 @@
 #define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_
 
 #include <locale>
+#include <optional>
 #include <sstream>
 #include <stdexcept>
 #include <string>
diff --git a/hardware_interface/src/lexical_casts.cpp b/hardware_interface/src/lexical_casts.cpp
index c9adcccf83..940beb6d0f 100644
--- a/hardware_interface/src/lexical_casts.cpp
+++ b/hardware_interface/src/lexical_casts.cpp
@@ -16,20 +16,42 @@
 
 namespace hardware_interface
 {
-double stod(const std::string & s)
+namespace impl
+{
+std::optional<double> stod(const std::string & s)
 {
+#if __cplusplus < 202002L
   // convert from string using no locale
+  // Impl with std::istringstream
   std::istringstream stream(s);
   stream.imbue(std::locale::classic());
   double result;
   stream >> result;
   if (stream.fail() || !stream.eof())
   {
-    throw std::invalid_argument("Failed converting string to real number");
+    return std::nullopt;
   }
   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;
+#endif
+}
+}  // namespace impl
+double stod(const std::string & s)
+{
+  if (const auto result = impl::stod(s))
+  {
+    return *result;
+  }
+  throw std::invalid_argument("Failed converting string to real number");
 }
-
 bool parse_bool(const std::string & bool_string)
 {
   return bool_string == "true" || bool_string == "True";