Skip to content

Commit

Permalink
Use locale independent conversion from double to string (#1099)
Browse files Browse the repository at this point in the history
  • Loading branch information
simonschmeisser authored and rhaschke committed Oct 26, 2018
1 parent 751b543 commit 2e6f28f
Show file tree
Hide file tree
Showing 7 changed files with 194 additions and 11 deletions.
3 changes: 3 additions & 0 deletions moveit_core/CMakeLists.txt
Expand Up @@ -90,6 +90,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
profiler/include
sensor_manager/include
trajectory_processing/include
utils/include
)

catkin_package(
Expand All @@ -115,6 +116,7 @@ catkin_package(
moveit_distance_field
moveit_kinematics_metrics
moveit_dynamics_solver
moveit_utils
${OCTOMAP_LIBRARIES}
CATKIN_DEPENDS
tf2_eigen
Expand Down Expand Up @@ -186,3 +188,4 @@ add_subdirectory(trajectory_processing)
add_subdirectory(distance_field)
add_subdirectory(kinematics_metrics)
add_subdirectory(dynamics_solver)
add_subdirectory(utils)
11 changes: 11 additions & 0 deletions 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})
72 changes: 72 additions & 0 deletions 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
91 changes: 91 additions & 0 deletions 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);
}
}
}
Expand Up @@ -45,6 +45,8 @@
#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>
#include <ompl/base/goals/GoalLazySamples.h>
Expand Down Expand Up @@ -242,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)
Expand All @@ -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
Expand Down
6 changes: 4 additions & 2 deletions moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
Expand Up @@ -39,6 +39,7 @@
#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>

ompl_interface::OMPLInterface::OMPLInterface(const robot_model::RobotModelConstPtr& kmodel, const ros::NodeHandle& nh)
Expand Down Expand Up @@ -201,7 +202,7 @@ bool ompl_interface::OMPLInterface::loadPlannerConfiguration(
if (element.second.getType() == XmlRpc::XmlRpcValue::TypeString)
planner_config.config[element.first] = static_cast<std::string>(element.second);
else if (element.second.getType() == XmlRpc::XmlRpcValue::TypeDouble)
planner_config.config[element.first] = std::to_string(static_cast<double>(element.second));
planner_config.config[element.first] = moveit::core::toString(static_cast<double>(element.second));
else if (element.second.getType() == XmlRpc::XmlRpcValue::TypeInt)
planner_config.config[element.first] = std::to_string(static_cast<int>(element.second));
else if (element.second.getType() == XmlRpc::XmlRpcValue::TypeBoolean)
Expand Down Expand Up @@ -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;
}

Expand Down
14 changes: 7 additions & 7 deletions moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
Expand Up @@ -35,6 +35,7 @@
/* Author: Ryan Luna */

#include <moveit/benchmarks/BenchmarkExecutor.h>
#include <moveit/utils/lexical_casts.h>
#include <moveit/version.h>
#include <tf2_eigen/tf2_eigen.h>

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
}
}

Expand Down

0 comments on commit 2e6f28f

Please sign in to comment.