From f1fa2bf8982123c909649644dd5ea90eaf4a4832 Mon Sep 17 00:00:00 2001
From: Ioan Sucan <isucan@willowgarage.com>
Date: Thu, 10 Nov 2011 00:21:38 +0000
Subject: [PATCH] make parsing of doubles slightly more robust

---
 srdf/CMakeLists.txt |  1 +
 srdf/src/model.cpp  | 19 ++++++++++++++-----
 2 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/srdf/CMakeLists.txt b/srdf/CMakeLists.txt
index 14a7449..ae9b903 100644
--- a/srdf/CMakeLists.txt
+++ b/srdf/CMakeLists.txt
@@ -10,6 +10,7 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 set(ROS_BUILD_TYPE Debug)
 
 rosbuild_init()
+rosbuild_add_boost_directories()
 
 #set the default path for built executables to the "bin" directory
 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
diff --git a/srdf/src/model.cpp b/srdf/src/model.cpp
index a70c99c..cc44fed 100644
--- a/srdf/src/model.cpp
+++ b/srdf/src/model.cpp
@@ -36,6 +36,7 @@
 
 #include "srdf/model.h"
 #include <ros/console.h>
+#include <boost/lexical_cast.hpp>
 #include <algorithm>
 #include <fstream>
 #include <sstream>
@@ -314,13 +315,21 @@ void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlE
                     continue;
                 }
             }
-            std::string jval_str = std::string(jval);
-            std::stringstream ss(jval_str);
-            while (ss.good() && !ss.eof())
+            try
             {
-                double val; ss >> val >> std::ws;
-                gs.joint_values_[jname_str].push_back(val);
+                std::string jval_str = std::string(jval);
+                std::stringstream ss(jval_str);
+                while (ss.good() && !ss.eof())
+                {
+                    std::string val; ss >> val >> std::ws;
+                    gs.joint_values_[jname_str].push_back(boost::lexical_cast<double>(val));
+                }
             }
+            catch (boost::bad_lexical_cast &e)
+            {
+                ROS_ERROR("Unable to parse joint value '%s'", jval);
+            }
+
             if (gs.joint_values_.empty())
                 ROS_ERROR("Unable to parse joint value ('%s') for joint '%s' in group state '%s'", jval, jname, sname);
         }