From 7e71811baea1bdffbf000d6ee7b4c7ce364d3853 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 29 Dec 2023 10:19:18 +0000 Subject: [PATCH 1/7] Use portable version for string-to-double conversion --- .../include/hardware_interface/tools.hpp | 47 +++++++++++++++++++ .../src/mock_components/generic_system.cpp | 18 ++----- 2 files changed, 51 insertions(+), 14 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/tools.hpp diff --git a/hardware_interface/include/hardware_interface/tools.hpp b/hardware_interface/include/hardware_interface/tools.hpp new file mode 100644 index 0000000000..ff98cb1157 --- /dev/null +++ b/hardware_interface/include/hardware_interface/tools.hpp @@ -0,0 +1,47 @@ +// 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__TOOLS_HPP_ +#define HARDWARE_INTERFACE__TOOLS_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::runtime_exception if not a valid number + * from + https://github.com/ros-planning/srdfdom/blob/ad17b8d25812f752c397a6011cec64aeff090c46/src/model.cpp#L53 +*/ +double toDouble(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; +} + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__TOOLS_HPP_ diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index f4aee6c8a6..f4bc0c6944 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -26,22 +26,12 @@ #include #include "hardware_interface/component_parser.hpp" +#include "hardware_interface/tools.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) { @@ -123,7 +113,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::toDouble(it->second); it = info_.hardware_parameters.find("custom_interface_with_following_offset"); if (it != info_.hardware_parameters.end()) { @@ -169,7 +159,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::toDouble(joint.parameters.at("multiplier")); } mimic_joints_.push_back(mimic_joint); } @@ -696,7 +686,7 @@ 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::toDouble(interface.initial_value); } } } From 10ccd99f892efa413c53832f7e54e50661752a22 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 29 Dec 2023 15:42:37 +0000 Subject: [PATCH 2/7] Rename file --- .../include/hardware_interface/{tools.hpp => helpers.hpp} | 6 +++--- hardware_interface/src/mock_components/generic_system.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) rename hardware_interface/include/hardware_interface/{tools.hpp => helpers.hpp} (91%) diff --git a/hardware_interface/include/hardware_interface/tools.hpp b/hardware_interface/include/hardware_interface/helpers.hpp similarity index 91% rename from hardware_interface/include/hardware_interface/tools.hpp rename to hardware_interface/include/hardware_interface/helpers.hpp index ff98cb1157..b21e51707e 100644 --- a/hardware_interface/include/hardware_interface/tools.hpp +++ b/hardware_interface/include/hardware_interface/helpers.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__TOOLS_HPP_ -#define HARDWARE_INTERFACE__TOOLS_HPP_ +#ifndef HARDWARE_INTERFACE__HELPERS_HPP_ +#define HARDWARE_INTERFACE__HELPERS_HPP_ #include #include @@ -44,4 +44,4 @@ double toDouble(const std::string & s) } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__TOOLS_HPP_ +#endif // HARDWARE_INTERFACE__HELPERS_HPP_ diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index f4bc0c6944..80b46b85d3 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -26,7 +26,7 @@ #include #include "hardware_interface/component_parser.hpp" -#include "hardware_interface/tools.hpp" +#include "hardware_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" From bab7a7befec8e4fdafba3edc034e317849352b71 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 29 Dec 2023 22:22:27 +0000 Subject: [PATCH 3/7] Fix API documentation --- hardware_interface/include/hardware_interface/helpers.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/helpers.hpp b/hardware_interface/include/hardware_interface/helpers.hpp index b21e51707e..d6d325e109 100644 --- a/hardware_interface/include/hardware_interface/helpers.hpp +++ b/hardware_interface/include/hardware_interface/helpers.hpp @@ -24,7 +24,7 @@ namespace hardware_interface { /** \brief Helper function to convert a std::string to double in a locale-independent way. - \throws std::runtime_exception if not a valid number + \throws std::invalid_argument if not a valid number * from https://github.com/ros-planning/srdfdom/blob/ad17b8d25812f752c397a6011cec64aeff090c46/src/model.cpp#L53 */ From f64dd7b1193737cec54e17b942135aeb1c8bb20b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 3 Jan 2024 19:22:25 +0000 Subject: [PATCH 4/7] Rename the file --- .../hardware_interface/{helpers.hpp => double_parsing.hpp} | 6 +++--- hardware_interface/src/mock_components/generic_system.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) rename hardware_interface/include/hardware_interface/{helpers.hpp => double_parsing.hpp} (90%) diff --git a/hardware_interface/include/hardware_interface/helpers.hpp b/hardware_interface/include/hardware_interface/double_parsing.hpp similarity index 90% rename from hardware_interface/include/hardware_interface/helpers.hpp rename to hardware_interface/include/hardware_interface/double_parsing.hpp index d6d325e109..a9bada368f 100644 --- a/hardware_interface/include/hardware_interface/helpers.hpp +++ b/hardware_interface/include/hardware_interface/double_parsing.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__HELPERS_HPP_ -#define HARDWARE_INTERFACE__HELPERS_HPP_ +#ifndef HARDWARE_INTERFACE__DOUBLE_PARSING_HPP_ +#define HARDWARE_INTERFACE__DOUBLE_PARSING_HPP_ #include #include @@ -44,4 +44,4 @@ double toDouble(const std::string & s) } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__HELPERS_HPP_ +#endif // HARDWARE_INTERFACE__DOUBLE_PARSING_HPP_ diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 80b46b85d3..4cfcf7fc45 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -26,7 +26,7 @@ #include #include "hardware_interface/component_parser.hpp" -#include "hardware_interface/helpers.hpp" +#include "hardware_interface/double_parsing.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" From ef446776af8e07df1c3768a8f31318e6ed482597 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 3 Jan 2024 19:47:08 +0000 Subject: [PATCH 5/7] Rename it to stod (like in std namespace) --- .../include/hardware_interface/double_parsing.hpp | 2 +- hardware_interface/src/mock_components/generic_system.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface/include/hardware_interface/double_parsing.hpp b/hardware_interface/include/hardware_interface/double_parsing.hpp index a9bada368f..2020191be6 100644 --- a/hardware_interface/include/hardware_interface/double_parsing.hpp +++ b/hardware_interface/include/hardware_interface/double_parsing.hpp @@ -28,7 +28,7 @@ namespace hardware_interface * from https://github.com/ros-planning/srdfdom/blob/ad17b8d25812f752c397a6011cec64aeff090c46/src/model.cpp#L53 */ -double toDouble(const std::string & s) +double stod(const std::string & s) { // convert from string using no locale std::istringstream stream(s); diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 4cfcf7fc45..e68aa2124b 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -113,7 +113,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_ = hardware_interface::toDouble(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()) { @@ -159,7 +159,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 = hardware_interface::toDouble(joint.parameters.at("multiplier")); + mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); } mimic_joints_.push_back(mimic_joint); } @@ -686,7 +686,7 @@ void GenericSystem::initialize_storage_vectors( // Check the initial_value param is used if (!interface.initial_value.empty()) { - states[index][i] = hardware_interface::toDouble(interface.initial_value); + states[index][i] = hardware_interface::stod(interface.initial_value); } } } From ad364013f1ead506e76384563b157f5738e105aa Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 3 Jan 2024 19:51:44 +0000 Subject: [PATCH 6/7] Move parse_bool to same file and rename it again --- .../include/hardware_interface/component_parser.hpp | 3 --- .../{double_parsing.hpp => lexical_casts.hpp} | 11 ++++++++--- hardware_interface/src/component_parser.cpp | 6 +----- .../src/mock_components/generic_system.cpp | 2 +- 4 files changed, 10 insertions(+), 12 deletions(-) rename hardware_interface/include/hardware_interface/{double_parsing.hpp => lexical_casts.hpp} (84%) 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/double_parsing.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp similarity index 84% rename from hardware_interface/include/hardware_interface/double_parsing.hpp rename to hardware_interface/include/hardware_interface/lexical_casts.hpp index 2020191be6..e0333756f4 100644 --- a/hardware_interface/include/hardware_interface/double_parsing.hpp +++ b/hardware_interface/include/hardware_interface/lexical_casts.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef HARDWARE_INTERFACE__DOUBLE_PARSING_HPP_ -#define HARDWARE_INTERFACE__DOUBLE_PARSING_HPP_ +#ifndef HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ +#define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ #include #include @@ -42,6 +42,11 @@ double stod(const std::string & s) return result; } +bool parse_bool(const std::string & bool_string) +{ + return bool_string == "true" || bool_string == "True"; +} + } // namespace hardware_interface -#endif // HARDWARE_INTERFACE__DOUBLE_PARSING_HPP_ +#endif // HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 8e9b6bf16b..6cd9e81f22 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 { @@ -616,9 +617,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 e68aa2124b..22d8aa573c 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -26,7 +26,7 @@ #include #include "hardware_interface/component_parser.hpp" -#include "hardware_interface/double_parsing.hpp" +#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" From 560605c8d73bba2fdaf1dbcca50a8f02cbe1404d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 3 Jan 2024 20:00:51 +0000 Subject: [PATCH 7/7] Another occurrence of from_chars --- hardware_interface/src/component_parser.cpp | 25 +++++++++------------ 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 6cd9e81f22..4f67d3e8b6 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -129,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(); }