Make double parsing locale independent (ros-controls#921)

hardware_interface/src/component_parser.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <tinyxml2.h>
+#include <charconv>
 #include <regex>
 #include <stdexcept>
 #include <string>
@@ -123,26 +124,30 @@ double get_parameter_value_or(
 {
   while (params_it)
   {
-    try
+    // Fill the map with parameters
+    const auto tag_name = params_it->Name();
+    if (strcmp(tag_name, parameter_name) == 0)
     {
-      // Fill the map with parameters
-      const auto tag_name = params_it->Name();
-      if (strcmp(tag_name, parameter_name) == 0)
+      const auto tag_text = params_it->GetText();
+      if (tag_text)
       {
-        const auto tag_text = params_it->GetText();
-        if (tag_text)
+        // 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())
         {
-          return std::stod(tag_text);
+          return result_value;
         }
+
+        // Parsing failed - exit loop and return default value
+        break;
       }
     }
-    catch (const std::exception & e)
-    {
-      return default_value;
-    }
 
     params_it = params_it->NextSiblingElement();
   }
+
   return default_value;
 }
 

hardware_interface/src/mock_components/generic_system.cpp

@@ -17,6 +17,7 @@
 #include "mock_components/generic_system.hpp"
 
 #include <algorithm>
+#include <charconv>
 #include <cmath>
 #include <iterator>
 #include <limits>
@@ -29,6 +30,18 @@
 
 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)
 {
   if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
@@ -101,7 +114,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_ = parse_double(it->second);
     it = info_.hardware_parameters.find("custom_interface_with_following_offset");
     if (it != info_.hardware_parameters.end())
     {
@@ -147,7 +160,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 = parse_double(joint.parameters.at("multiplier"));
       }
       mimic_joints_.push_back(mimic_joint);
     }
@@ -455,15 +468,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] = parse_double(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] = parse_double(it2->second);
             print_hint = true;
           }
           else

hardware_interface/test/test_component_parser.cpp

@@ -510,6 +510,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 =