diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index b8c5d2f236..3426806425 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -90,6 +90,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS profiler/include sensor_manager/include trajectory_processing/include + utils/include ) catkin_package( @@ -115,6 +116,7 @@ catkin_package( moveit_distance_field moveit_kinematics_metrics moveit_dynamics_solver + moveit_utils ${OCTOMAP_LIBRARIES} CATKIN_DEPENDS tf2_eigen @@ -186,3 +188,4 @@ add_subdirectory(trajectory_processing) add_subdirectory(distance_field) add_subdirectory(kinematics_metrics) add_subdirectory(dynamics_solver) +add_subdirectory(utils) diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt new file mode 100644 index 0000000000..547b00f7ca --- /dev/null +++ b/moveit_core/utils/CMakeLists.txt @@ -0,0 +1,11 @@ +set(MOVEIT_LIB_NAME moveit_utils) + +add_library(${MOVEIT_LIB_NAME} src/lexical_casts.cpp) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) + +target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +install(TARGETS ${MOVEIT_LIB_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_core/utils/include/moveit/utils/lexical_casts.h b/moveit_core/utils/include/moveit/utils/lexical_casts.h new file mode 100644 index 0000000000..ce0fbcb5b5 --- /dev/null +++ b/moveit_core/utils/include/moveit/utils/lexical_casts.h @@ -0,0 +1,72 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, isys vision, GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Simon Schmeisser */ + +#ifndef MOVEIT_CORE_UTILS_LEXICAL_CASTS_ +#define MOVEIT_CORE_UTILS_LEXICAL_CASTS_ + +/** \file lexical_casts.h + * \brief locale-agnostic conversion functions from floating point numbers to strings + * + * Depending on the system locale, a different decimal seperator might be used + * for floating point numbers. This is often not wanted for internal (ie non-user + * facing) purposes. This module provides conversion functions that use std::locale::classic() + * (i.e. the default if no locale is set on the system). + */ + +#include +namespace moveit +{ +namespace core +{ +/** \brief Convert a double to std::string using the classic C locale */ +std::string toString(double d); + +/** \brief Convert a float to std::string using the classic C locale */ +std::string toString(float f); + +/** \brief Converts a std::string to double using the classic C locale + \throws std::runtime_exception if not a valid number +*/ +double toDouble(const std::string& s); + +/** \brief Converts a std::string to float using the classic C locale + \throws std::runtime_exception if not a valid number +*/ +float toFloat(const std::string& s); +} +} + +#endif diff --git a/moveit_core/utils/src/lexical_casts.cpp b/moveit_core/utils/src/lexical_casts.cpp new file mode 100644 index 0000000000..7bda270c4b --- /dev/null +++ b/moveit_core/utils/src/lexical_casts.cpp @@ -0,0 +1,91 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, isys vision, GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Simon Schmeisser */ + +#include "moveit/utils/lexical_casts.h" + +#include +#include + +namespace moveit +{ +namespace core +{ +template +std::string toStringImpl(InType t) +{ + // convert to string using no locale + std::ostringstream oss; + oss.imbue(std::locale::classic()); + oss << t; + return oss.str(); +} + +std::string toString(double d) +{ + return toStringImpl(d); +} + +std::string toString(float f) +{ + return toStringImpl(f); +} + +template +OutType toRealImpl(const std::string& s) +{ + // convert from string using no locale + std::istringstream stream(s); + stream.imbue(std::locale::classic()); + OutType result; + stream >> result; + if (stream.fail() || !stream.eof()) + { + throw std::runtime_error("Failed converting string to real number"); + } + return result; +} + +double toDouble(const std::string& s) +{ + return toRealImpl(s); +} + +float toFloat(const std::string& s) +{ + return toRealImpl(s); +} +} +} diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 9923ace2fb..d08d14b4a0 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -45,6 +45,8 @@ #include #include #include +#include +#include #include #include @@ -242,7 +244,7 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() { // clang-format off double longest_valid_segment_fraction_config = (it != cfg.end()) - ? boost::lexical_cast(it->second) // value from config file if there + ? moveit::core::toDouble(it->second) // value from config file if there : 0.01; // default value in OMPL. double longest_valid_segment_fraction_final = longest_valid_segment_fraction_config; if (max_solution_segment_length_ > 0.0) @@ -255,7 +257,9 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() ); } // clang-format on - cfg["longest_valid_segment_fraction"] = std::to_string(longest_valid_segment_fraction_final); + + // convert to string using no locale + cfg["longest_valid_segment_fraction"] = moveit::core::toString(longest_valid_segment_fraction_final); } // set the projection evaluator diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index a5a41dd4d5..b7df9a71d3 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& kmodel, const ros::NodeHandle& nh) @@ -201,7 +202,7 @@ bool ompl_interface::OMPLInterface::loadPlannerConfiguration( if (element.second.getType() == XmlRpc::XmlRpcValue::TypeString) planner_config.config[element.first] = static_cast(element.second); else if (element.second.getType() == XmlRpc::XmlRpcValue::TypeDouble) - planner_config.config[element.first] = std::to_string(static_cast(element.second)); + planner_config.config[element.first] = moveit::core::toString(static_cast(element.second)); else if (element.second.getType() == XmlRpc::XmlRpcValue::TypeInt) planner_config.config[element.first] = std::to_string(static_cast(element.second)); else if (element.second.getType() == XmlRpc::XmlRpcValue::TypeBoolean) @@ -240,7 +241,8 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations() double value_d; if (nh_.getParam(group_name + "/" + k, value_d)) { - specific_group_params[k] = std::to_string(value_d); + // convert to string using no locale + specific_group_params[k] = moveit::core::toString(value_d); continue; } diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index ec7128cafd..c8056d25e8 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -35,6 +35,7 @@ /* Author: Ryan Luna */ #include +#include #include #include @@ -819,7 +820,7 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, const planning_interface::MotionPlanDetailedResponse& mp_res, bool solved, double total_time) { - metrics["time REAL"] = boost::lexical_cast(total_time); + metrics["time REAL"] = moveit::core::toString(total_time); metrics["solved BOOLEAN"] = boost::lexical_cast(solved); if (solved) @@ -892,16 +893,15 @@ void BenchmarkExecutor::collectMetrics(PlannerRunData& metrics, smoothness /= (double)p.getWayPointCount(); } metrics["path_" + mp_res.description_[j] + "_correct BOOLEAN"] = boost::lexical_cast(correct); - metrics["path_" + mp_res.description_[j] + "_length REAL"] = boost::lexical_cast(L); - metrics["path_" + mp_res.description_[j] + "_clearance REAL"] = boost::lexical_cast(clearance); - metrics["path_" + mp_res.description_[j] + "_smoothness REAL"] = boost::lexical_cast(smoothness); - metrics["path_" + mp_res.description_[j] + "_time REAL"] = - boost::lexical_cast(mp_res.processing_time_[j]); + metrics["path_" + mp_res.description_[j] + "_length REAL"] = moveit::core::toString(L); + metrics["path_" + mp_res.description_[j] + "_clearance REAL"] = moveit::core::toString(clearance); + metrics["path_" + mp_res.description_[j] + "_smoothness REAL"] = moveit::core::toString(smoothness); + metrics["path_" + mp_res.description_[j] + "_time REAL"] = moveit::core::toString(mp_res.processing_time_[j]); process_time -= mp_res.processing_time_[j]; } if (process_time <= 0.0) process_time = 0.0; - metrics["process_time REAL"] = boost::lexical_cast(process_time); + metrics["process_time REAL"] = moveit::core::toString(process_time); } }