Use portable version for string-to-double conversion (backport #1257) (…

…#1269)

hardware_interface/include/hardware_interface/component_parser.hpp

@@ -33,8 +33,5 @@ namespace hardware_interface
 HARDWARE_INTERFACE_PUBLIC
 std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string & urdf);
 
-HARDWARE_INTERFACE_PUBLIC
-bool parse_bool(const std::string & bool_string);
-
 }  // namespace hardware_interface
 #endif  // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_

hardware_interface/include/hardware_interface/lexical_casts.hpp

@@ -0,0 +1,52 @@
+// Copyright 2023 ros2_control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_
+#define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_
+
+#include <locale>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+
+namespace hardware_interface
+{
+
+/** \brief Helper function to convert a std::string to double in a locale-independent way.
+ \throws std::invalid_argument if not a valid number
+ * from
+ https://github.com/ros-planning/srdfdom/blob/ad17b8d25812f752c397a6011cec64aeff090c46/src/model.cpp#L53
+*/
+double stod(const std::string & s)
+{
+  // convert from string using no locale
+  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 result;
+}
+
+bool parse_bool(const std::string & bool_string)
+{
+  return bool_string == "true" || bool_string == "True";
+}
+
+}  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_

hardware_interface/src/component_parser.cpp

@@ -23,6 +23,7 @@
 
 #include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/lexical_casts.hpp"
 
 namespace
 {
@@ -128,26 +129,23 @@ double get_parameter_value_or(
 {
   while (params_it)
   {
-    // Fill the map with parameters
-    const auto tag_name = params_it->Name();
-    if (strcmp(tag_name, parameter_name) == 0)
+    try
     {
-      const auto tag_text = params_it->GetText();
-      if (tag_text)
+      // Fill the map with parameters
+      const auto tag_name = params_it->Name();
+      if (strcmp(tag_name, parameter_name) == 0)
       {
-        // Parse and return double value if there is no parsing error
-        double result_value;
-        const auto parse_result =
-          std::from_chars(tag_text, tag_text + std::strlen(tag_text), result_value);
-        if (parse_result.ec == std::errc())
+        const auto tag_text = params_it->GetText();
+        if (tag_text)
         {
-          return result_value;
+          return hardware_interface::stod(tag_text);
         }
-
-        // Parsing failed - exit loop and return default value
-        break;
       }
     }
+    catch (const std::exception & e)
+    {
+      return default_value;
+    }
 
     params_it = params_it->NextSiblingElement();
   }
@@ -616,9 +614,4 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
   return hardware_info;
 }
 
-bool parse_bool(const std::string & bool_string)
-{
-  return bool_string == "true" || bool_string == "True";
-}
-
 }  // namespace hardware_interface

hardware_interface/src/mock_components/generic_system.cpp

@@ -26,22 +26,12 @@
 #include <vector>
 
 #include "hardware_interface/component_parser.hpp"
+#include "hardware_interface/lexical_casts.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rcutils/logging_macros.h"
 
 namespace mock_components
 {
-double parse_double(const std::string & text)
-{
-  double result_value;
-  const auto parse_result = std::from_chars(text.data(), text.data() + text.size(), result_value);
-  if (parse_result.ec == std::errc())
-  {
-    return result_value;
-  }
-
-  return 0.0;
-}
 
 CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info)
 {
@@ -149,7 +139,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
   it = info_.hardware_parameters.find("position_state_following_offset");
   if (it != info_.hardware_parameters.end())
   {
-    position_state_following_offset_ = parse_double(it->second);
+    position_state_following_offset_ = hardware_interface::stod(it->second);
     it = info_.hardware_parameters.find("custom_interface_with_following_offset");
     if (it != info_.hardware_parameters.end())
     {
@@ -195,7 +185,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
       auto param_it = joint.parameters.find("multiplier");
       if (param_it != joint.parameters.end())
       {
-        mimic_joint.multiplier = parse_double(joint.parameters.at("multiplier"));
+        mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier"));
       }
       mimic_joints_.push_back(mimic_joint);
     }
@@ -723,15 +713,15 @@ void GenericSystem::initialize_storage_vectors(
         // Check the initial_value param is used
         if (!interface.initial_value.empty())
         {
-          states[index][i] = parse_double(interface.initial_value);
+          states[index][i] = hardware_interface::stod(interface.initial_value);
         }
         else
         {
           // Initialize the value in old way with warning message
           auto it2 = component.parameters.find("initial_" + interface.name);
           if (it2 != component.parameters.end())
           {
-            states[index][i] = parse_double(it2->second);
+            states[index][i] = hardware_interface::stod(it2->second);
             print_hint = true;
           }
           else