From 945d64ed3640e8fcda6050958e020ca17ef1eb8b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 4 Jan 2024 17:16:03 +0100 Subject: [PATCH] Use portable version for string-to-double conversion (backport #1257) (#1268) --- .../hardware_interface/component_parser.hpp | 3 -- .../hardware_interface/lexical_casts.hpp | 52 +++++++++++++++++++ hardware_interface/src/component_parser.cpp | 8 +-- .../src/mock_components/generic_system.cpp | 9 ++-- .../test/test_component_parser.cpp | 24 +++++++++ 5 files changed, 83 insertions(+), 13 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/lexical_casts.hpp diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 5112f7927e..d5d999cca8 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,8 +33,5 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector 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_ diff --git a/hardware_interface/include/hardware_interface/lexical_casts.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp new file mode 100644 index 0000000000..e0333756f4 --- /dev/null +++ b/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 +#include +#include +#include + +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_ diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index c05ec8ca47..da83d69bee 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/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 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 diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 08132dbdf6..0a88be99cd 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -25,6 +25,7 @@ #include #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,7 +711,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] = hardware_interface::stod(interface.initial_value); } else { @@ -718,7 +719,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] = hardware_interface::stod(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 b19c10169e..cdd2f2bb17 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/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 =