diff --git a/hardware_interface/include/hardware_interface/tools.hpp b/hardware_interface/include/hardware_interface/tools.hpp new file mode 100644 index 00000000000..ff98cb1157a --- /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 f4aee6c8a6d..f4bc0c6944b 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); } } }