Simplify visual sensor parsing

Move basic attribute parsing routine to utils.h

Author: rhaschke

urdf_parser/include/urdf_parser/utils.h

@@ -0,0 +1,88 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2016, CITEC, Bielefeld University
+*  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 the copyright holder 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: Robert Haschke */
+
+#ifndef URDF_PARSER_UTIL_H
+#define URDF_PARSER_UTIL_H
+
+#include <urdf_exception/exception.h>
+#include <urdf_model/utils.h>
+
+namespace urdf {
+
+template <typename T>
+T parseAttribute(const char* value);
+
+template<>
+inline std::string parseAttribute<std::string>(const char* value)
+{
+  return value;
+}
+
+template<>
+inline double parseAttribute<double>(const char* value)
+{
+  return strToDouble(value);
+}
+
+template<>
+inline unsigned int parseAttribute<unsigned int>(const char* value)
+{
+  return std::stoul(value);
+}
+
+template <typename T>
+T parseAttribute(const TiXmlElement &tag, const char* attr, const T* default_value=NULL)
+{
+  const char* value = tag.Attribute(attr);
+  if (!value)
+  {
+    if (default_value) return *default_value;
+    else throw ParseError(std::string("missing '") + attr + "'' attribute");
+  }
+
+  try
+  {
+    return parseAttribute<T>(value);
+  }
+  catch (const std::exception &e)
+  {
+    throw ParseError(std::string("failed to parse '") + attr + "' attribute: " + e.what());
+  }
+}
+
+}
+
+#endif

urdf_parser/src/sensor_parser.cpp

@@ -36,11 +36,10 @@
 
 
 #include "urdf_parser/sensor_parser.h"
+
 #include "urdf_parser/pose.h"
 #include <urdf_sensor/camera.h>
 #include <urdf_sensor/ray.h>
-
-#include <boost/lexical_cast.hpp>
 #include <console_bridge/console.h>
 
 namespace urdf {

urdf_parser/src/visual_sensor_parsers.cpp

@@ -35,259 +35,82 @@
 /* Author: John Hsu */
 
 #include "urdf_parser/visual_sensor_parsers.h"
+#include "urdf_parser/utils.h"
+#include "urdf_parser/pose.h"
 #include <urdf_sensor/camera.h>
 #include <urdf_sensor/ray.h>
 
-#include <fstream>
-#include <locale>
-#include <sstream>
-#include <stdexcept>
-#include <string>
-#include <algorithm>
-#include <tinyxml.h>
 #include <console_bridge/console.h>
-#include "urdf_parser/pose.h"
 
 namespace urdf {
 
 SensorBaseSharedPtr CameraParser::parse(TiXmlElement &config)
 {
-  CameraSharedPtr camera(new Camera());
-
   TiXmlElement *image = config.FirstChildElement("image");
   if (image)
   {
-    const char* width_char = image->Attribute("width");
-    if (width_char)
-    {
-      try
-      {
-        camera->width = std::stoul(width_char);
-      }
-      catch (std::invalid_argument &e)
-      {
-        CONSOLE_BRIDGE_logError("Camera image width [%s] is not a valid int: %s", width_char, e.what());
-        return CameraSharedPtr();
-      }
-      catch (std::out_of_range &e)
-      {
-        CONSOLE_BRIDGE_logError("Camera image width [%s] is out of range: %s", width_char, e.what());
-        return CameraSharedPtr();
-      }
-    }
-    else
-    {
-      CONSOLE_BRIDGE_logError("Camera sensor needs an image width attribute");
-      return CameraSharedPtr();
-    }
-
-    const char* height_char = image->Attribute("height");
-    if (height_char)
-    {
-      try
-      {
-        camera->height = std::stoul(height_char);
-      }
-      catch (std::invalid_argument &e)
-      {
-        CONSOLE_BRIDGE_logError("Camera image height [%s] is not a valid int: %s", height_char, e.what());
-        return CameraSharedPtr();
-      }
-      catch (std::out_of_range &e)
-      {
-        CONSOLE_BRIDGE_logError("Camera image height [%s] is out of range: %s", height_char, e.what());
-        return CameraSharedPtr();
-      }
-    }
-    else
-    {
-      CONSOLE_BRIDGE_logError("Camera sensor needs an image height attribute");
-      return CameraSharedPtr();
-    }
-
-    const char* format_char = image->Attribute("format");
-    if (format_char)
-      camera->format = std::string(format_char);
-    else
-    {
-      CONSOLE_BRIDGE_logError("Camera sensor needs an image format attribute");
-      return CameraSharedPtr();
-    }
-
-    const char* hfov_char = image->Attribute("hfov");
-    if (hfov_char)
-    {
-      try {
-        camera->hfov = strToDouble(hfov_char);
-      } catch(std::runtime_error &) {
-        CONSOLE_BRIDGE_logError("Camera image hfov [%s] is not a valid float", hfov_char);
-        return CameraSharedPtr();
-      }
-    }
-    else
-    {
-      CONSOLE_BRIDGE_logError("Camera sensor needs an image hfov attribute");
-      return CameraSharedPtr();
-    }
-
-    const char* near_char = image->Attribute("near");
-    if (near_char)
-    {
-      try {
-        camera->near = strToDouble(near_char);
-      } catch(std::runtime_error &) {
-        CONSOLE_BRIDGE_logError("Camera image near [%s] is not a valid float", near_char);
-        return CameraSharedPtr();
-      }
-    }
-    else
-    {
-      CONSOLE_BRIDGE_logError("Camera sensor needs an image near attribute");
+    try {
+      CameraSharedPtr camera(new Camera());
+      camera->width = parseAttribute<unsigned int>(*image, "width");
+      camera->height = parseAttribute<unsigned int>(*image, "height");
+      camera->format = parseAttribute<std::string>(*image, "format");
+      camera->hfov = parseAttribute<double>(*image, "hfov");
+      camera->near = parseAttribute<double>(*image, "near");
+      camera->far = parseAttribute<double>(*image, "far");
+      return camera;
+    }
+    catch (const std::exception &e)
+    {
+      CONSOLE_BRIDGE_logError("Camera sensor %s", e.what());
       return CameraSharedPtr();
     }
-
-    const char* far_char = image->Attribute("far");
-    if (far_char)
-    {
-      try {
-        camera->far = strToDouble(far_char);
-      } catch(std::runtime_error &) {
-        CONSOLE_BRIDGE_logError("Camera image far [%s] is not a valid float", far_char);
-        return CameraSharedPtr();
-      }
-    }
-    else
-    {
-      CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute");
-      return CameraSharedPtr();
-    }
-
   }
   else
   {
     CONSOLE_BRIDGE_logError("Camera sensor has no <image> element");
     return CameraSharedPtr();
   }
-  return camera;
 }
 
 
 SensorBaseSharedPtr RayParser::parse(TiXmlElement &config)
 {
-  RaySharedPtr ray (new Ray());
-
   TiXmlElement *horizontal = config.FirstChildElement("horizontal");
   if (horizontal)
   {
-    const char* samples_char = horizontal->Attribute("samples");
-    if (samples_char)
-    {
-      try
-      {
-        ray->horizontal_samples = std::stoul(samples_char);
-      }
-      catch (std::invalid_argument &e)
-      {
-        CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what());
-        return RaySharedPtr();
-      }
-      catch (std::out_of_range &e)
-      {
-        CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is out of range: %s", samples_char, e.what());
-        return RaySharedPtr();
-      }
-    }
-
-    const char* resolution_char = horizontal->Attribute("resolution");
-    if (resolution_char)
-    {
-      try {
-        ray->horizontal_resolution = strToDouble(resolution_char);
-      } catch(std::runtime_error &) {
-        CONSOLE_BRIDGE_logError("Ray horizontal resolution [%s] is not a valid float", resolution_char);
-        return RaySharedPtr();
-      }
+    try {
+      RaySharedPtr ray (new Ray());
+      ray->horizontal_samples = parseAttribute<unsigned int>(*horizontal, "samples");
+      ray->horizontal_resolution = parseAttribute<double>(*horizontal, "resolution");
+      ray->horizontal_min_angle = parseAttribute<double>(*horizontal, "min_angle");
+      ray->horizontal_max_angle = parseAttribute<double>(*horizontal, "max_angle");
+      return ray;
     }
-
-    const char* min_angle_char = horizontal->Attribute("min_angle");
-    if (min_angle_char)
+    catch (const std::exception &e)
     {
-      try {
-        ray->horizontal_min_angle = strToDouble(min_angle_char);
-      } catch(std::runtime_error &) {
-        CONSOLE_BRIDGE_logError("Ray horizontal min_angle [%s] is not a valid float", min_angle_char);
-        return RaySharedPtr();
-      }
-    }
-
-    const char* max_angle_char = horizontal->Attribute("max_angle");
-    if (max_angle_char)
-    {
-      try {
-        ray->horizontal_max_angle = strToDouble(max_angle_char);
-      } catch(std::runtime_error &) {
-        CONSOLE_BRIDGE_logError("Ray horizontal max_angle [%s] is not a valid float", max_angle_char);
-        return RaySharedPtr();
-      }
+      CONSOLE_BRIDGE_logError("Ray horizontal: %s", e.what());
+      return RaySharedPtr();
     }
   }
 
   TiXmlElement *vertical = config.FirstChildElement("vertical");
   if (vertical)
   {
-    const char* samples_char = vertical->Attribute("samples");
-    if (samples_char)
-    {
-      try
-      {
-        ray->vertical_samples = std::stoul(samples_char);
-      }
-      catch (std::invalid_argument &e)
-      {
-        CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what());
-        return RaySharedPtr();
-      }
-      catch (std::out_of_range &e)
-      {
-        CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is out of range: %s", samples_char, e.what());
-        return RaySharedPtr();
-      }
+    try {
+      RaySharedPtr ray (new Ray());
+      ray->vertical_samples = parseAttribute<unsigned int>(*vertical, "samples");
+      ray->vertical_resolution = parseAttribute<double>(*vertical, "resolution");
+      ray->vertical_min_angle = parseAttribute<double>(*vertical, "min_angle");
+      ray->vertical_max_angle = parseAttribute<double>(*vertical, "max_angle");
+      return ray;
     }
-
-    const char* resolution_char = vertical->Attribute("resolution");
-    if (resolution_char)
-    {
-      try {
-        ray->vertical_resolution = strToDouble(resolution_char);
-      } catch(std::runtime_error &) {
-        CONSOLE_BRIDGE_logError("Ray vertical resolution [%s] is not a valid float", resolution_char);
-        return RaySharedPtr();
-      }
-    }
-
-    const char* min_angle_char = vertical->Attribute("min_angle");
-    if (min_angle_char)
-    {
-      try {
-        ray->vertical_min_angle = strToDouble(min_angle_char);
-      } catch(std::runtime_error &) {
-        CONSOLE_BRIDGE_logError("Ray vertical min_angle [%s] is not a valid float", min_angle_char);
-        return RaySharedPtr();
-      }
-    }
-
-    const char* max_angle_char = vertical->Attribute("max_angle");
-    if (max_angle_char)
+    catch (const std::exception &e)
     {
-      try {
-        ray->vertical_max_angle = strToDouble(max_angle_char);
-      } catch(std::runtime_error &) {
-        CONSOLE_BRIDGE_logError("Ray vertical max_angle [%s] is not a valid float", max_angle_char);
-        return RaySharedPtr();
-      }
+      CONSOLE_BRIDGE_logError("Ray horizontal: %s", e.what());
+      return RaySharedPtr();
     }
   }
-  return ray;
+  return RaySharedPtr();
 }
 
 }