From 78bb0cacb4434006f4e6ed13a94a77bf2261a489 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Tue, 24 Jan 2023 18:56:41 +0100 Subject: [PATCH] Make double parsing locale independent (#921) --- hardware_interface/src/component_parser.cpp | 27 +++++++++++-------- .../src/mock_components/generic_system.cpp | 21 ++++++++++++--- .../test/test_component_parser.cpp | 24 +++++++++++++++++ 3 files changed, 57 insertions(+), 15 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 0dc4c1913d..cdd4206a04 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -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; } diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 244fdcd58d..d1405dac8c 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -17,6 +17,7 @@ #include "mock_components/generic_system.hpp" #include +#include #include #include #include @@ -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,7 +468,7 @@ 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 { @@ -463,7 +476,7 @@ void GenericSystem::initialize_storage_vectors( 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 diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index e6409b3d58..0941b7b8a7 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/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 =