Use locale independent conversion from double to string

moveit_core/CMakeLists.txt

@@ -89,6 +89,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
     profiler/include
     sensor_manager/include
     trajectory_processing/include
+    utils/include
 )
 
 catkin_package(
@@ -114,6 +115,7 @@ catkin_package(
     moveit_distance_field
     moveit_kinematics_metrics
     moveit_dynamics_solver
+    moveit_utils
     ${OCTOMAP_LIBRARIES}
   CATKIN_DEPENDS
     eigen_conversions
@@ -185,3 +187,4 @@ add_subdirectory(trajectory_processing)
 add_subdirectory(distance_field)
 add_subdirectory(kinematics_metrics)
 add_subdirectory(dynamics_solver)
+add_subdirectory(utils)

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})

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 <string>
+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

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 <locale>
+#include <sstream>
+
+namespace moveit
+{
+namespace core
+{
+template <class InType>
+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 <class OutType>
+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<double>(s);
+}
+
+float toFloat(const std::string& s)
+{
+  return toRealImpl<float>(s);
+}
+}
+}

moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp

@@ -45,6 +45,7 @@
 #include <moveit/ompl_interface/constraints_library.h>
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/profiler/profiler.h>
+#include <moveit/utils/lexical_casts.h>
 #include <eigen_conversions/eigen_msg.h>
 
 #include <ompl/base/samplers/UniformValidStateSampler.h>
@@ -243,7 +244,7 @@ void ompl_interface::ModelBasedPlanningContext::useConfig()
   {
     // clang-format off
     double longest_valid_segment_fraction_config = (it != cfg.end())
-      ? boost::lexical_cast<double>(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)
@@ -256,7 +257,9 @@ void ompl_interface::ModelBasedPlanningContext::useConfig()
       );
     }
     // clang-format on
-    cfg["longest_valid_segment_fraction"] = boost::lexical_cast<std::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

moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp

@@ -39,7 +39,9 @@
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/ompl_interface/detail/constrained_valid_state_sampler.h>
 #include <moveit/profiler/profiler.h>
+#include <moveit/utils/lexical_casts.h>
 #include <fstream>
+#include <locale>
 
 ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& kmodel, const ros::NodeHandle& nh)
   : nh_(nh)
@@ -204,9 +206,9 @@ bool ompl_interface::OMPLInterface::loadPlannerConfiguration(
     if (it->second.getType() == XmlRpc::XmlRpcValue::TypeString)
       planner_config.config[it->first] = static_cast<std::string>(it->second);
     else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeDouble)
-      planner_config.config[it->first] = boost::lexical_cast<std::string>(static_cast<double>(it->second));
+      planner_config.config[it->first] = moveit::core::toString(static_cast<double>(it->second));
     else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeInt)
-      planner_config.config[it->first] = boost::lexical_cast<std::string>(static_cast<int>(it->second));
+      planner_config.config[it->first] = std::to_string(static_cast<int>(it->second));
     else if (it->second.getType() == XmlRpc::XmlRpcValue::TypeBoolean)
       planner_config.config[it->first] = boost::lexical_cast<std::string>(static_cast<bool>(it->second));
   }
@@ -244,14 +246,15 @@ void ompl_interface::OMPLInterface::loadPlannerConfigurations()
         double value_d;
         if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_d))
         {
-          specific_group_params[KNOWN_GROUP_PARAMS[k]] = boost::lexical_cast<std::string>(value_d);
+          // convert to string using no locale
+          specific_group_params[KNOWN_GROUP_PARAMS[k]] = moveit::core::toString(value_d);
           continue;
         }
 
         int value_i;
         if (nh_.getParam(group_names[i] + "/" + KNOWN_GROUP_PARAMS[k], value_i))
         {
-          specific_group_params[KNOWN_GROUP_PARAMS[k]] = boost::lexical_cast<std::string>(value_i);
+          specific_group_params[KNOWN_GROUP_PARAMS[k]] = std::to_string(value_i);
           continue;
         }
 

moveit_ros/benchmarks/src/BenchmarkExecutor.cpp

@@ -35,6 +35,7 @@
 /* Author: Ryan Luna */
 
 #include <moveit/benchmarks/BenchmarkExecutor.h>
+#include <moveit/utils/lexical_casts.h>
 #include <moveit/version.h>
 #include <eigen_conversions/eigen_msg.h>
 
@@ -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<std::string>(total_time);
+  metrics["time REAL"] = moveit::core::toString(total_time);
   metrics["solved BOOLEAN"] = boost::lexical_cast<std::string>(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<std::string>(correct);
-      metrics["path_" + mp_res.description_[j] + "_length REAL"] = boost::lexical_cast<std::string>(L);
-      metrics["path_" + mp_res.description_[j] + "_clearance REAL"] = boost::lexical_cast<std::string>(clearance);
-      metrics["path_" + mp_res.description_[j] + "_smoothness REAL"] = boost::lexical_cast<std::string>(smoothness);
-      metrics["path_" + mp_res.description_[j] + "_time REAL"] =
-          boost::lexical_cast<std::string>(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<std::string>(process_time);
+    metrics["process_time REAL"] = moveit::core::toString(process_time);
   }
 }