From 0dfbd3dfe160d8ea0e8403955a794f94766e4443 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 4 Jan 2024 16:55:25 +0100 Subject: [PATCH] Use portable version for string-to-double conversion (#1257) --- .../hardware_interface/component_parser.hpp | 3 -- .../hardware_interface/lexical_casts.hpp | 52 +++++++++++++++++++ hardware_interface/src/component_parser.cpp | 31 +++++------ .../src/mock_components/generic_system.cpp | 18 ++----- 4 files changed, 68 insertions(+), 36 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 8e9b6bf16b..4f67d3e8b6 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 { @@ -128,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(); } @@ -616,9 +614,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 f4aee6c8a6..22d8aa573c 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/lexical_casts.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::stod(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::stod(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::stod(interface.initial_value); } } }