Merge branch 'master' into allow-prepare-comand-mode-switch-only-for-…

…existing-and-available-interfaces

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>

hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp

@@ -17,13 +17,13 @@
 
 namespace hardware_interface
 {
-/// Constant defining position interface
+/// Constant defining position interface name
 constexpr char HW_IF_POSITION[] = "position";
-/// Constant defining velocity interface
+/// Constant defining velocity interface name
 constexpr char HW_IF_VELOCITY[] = "velocity";
-/// Constant defining acceleration interface
+/// Constant defining acceleration interface name
 constexpr char HW_IF_ACCELERATION[] = "acceleration";
-/// Constant defining effort interface
+/// Constant defining effort interface name
 constexpr char HW_IF_EFFORT[] = "effort";
 }  // namespace hardware_interface
 

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";