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

…#1268)

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
 {
@@ -135,7 +136,7 @@ double get_parameter_value_or(
         const auto tag_text = params_it->GetText();
         if (tag_text)
         {
-          return std::stod(tag_text);
+          return hardware_interface::stod(tag_text);
         }
       }
     }
@@ -593,9 +594,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

@@ -25,6 +25,7 @@
 #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"
 
@@ -136,7 +137,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_ = std::stod(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())
     {
@@ -182,7 +183,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 = std::stod(joint.parameters.at("multiplier"));
+        mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier"));
       }
       mimic_joints_.push_back(mimic_joint);
     }
@@ -710,15 +711,15 @@ void GenericSystem::initialize_storage_vectors(
         // Check the initial_value param is used
         if (!interface.initial_value.empty())
         {
-          states[index][i] = std::stod(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] = std::stod(it2->second);
+            states[index][i] = hardware_interface::stod(it2->second);
             print_hint = true;
           }
           else

hardware_interface/test/test_component_parser.cpp

@@ -500,6 +500,30 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only)
   EXPECT_EQ(transmission.parameters.at("additional_special_parameter"), "1337");
 }
 
+TEST_F(TestComponentParser, successfully_parse_locale_independent_double)
+{
+  // Set to locale with comma-separated decimals
+  std::setlocale(LC_NUMERIC, "de_DE.UTF-8");
+
+  std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) +
+                             ros2_control_test_assets::valid_urdf_ros2_control_actuator_only +
+                             ros2_control_test_assets::urdf_tail;
+
+  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
+  ASSERT_THAT(control_hardware, SizeIs(1));
+  const auto hardware_info = control_hardware.at(0);
+
+  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.13");
+
+  ASSERT_THAT(hardware_info.transmissions, SizeIs(1));
+  const auto transmission = hardware_info.transmissions[0];
+  EXPECT_THAT(transmission.joints, SizeIs(1));
+  const auto joint = transmission.joints[0];
+
+  // Test that we still parse doubles using dot notation
+  EXPECT_THAT(joint.mechanical_reduction, DoubleEq(325.949));
+}
+
 TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio)
 {
   std::string urdf_to_test =