From 499de2b01ed39a0c107202c1a32b2a5715f52a54 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 16 Mar 2016 21:00:57 +0100 Subject: [PATCH] simplified visual sensor parsing moved basic attribute parsing routine to utils.h --- urdf_parser/CMakeLists.txt | 2 +- urdf_parser/include/urdf_parser/utils.h | 73 +++++++ urdf_parser/src/sensor_parser.cpp | 3 +- urdf_parser/src/visual_sensor_parsers.cpp | 234 +++------------------- 4 files changed, 102 insertions(+), 210 deletions(-) create mode 100644 urdf_parser/include/urdf_parser/utils.h diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index baf284cf..387af47e 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -10,7 +10,7 @@ set_target_properties(urdfdom_model PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERS add_library(urdfdom_sensor SHARED src/sensor_parser.cpp src/visual_sensor_parsers.cpp - include/urdf_parser/sensor_parser.h include/urdf_parser/visual_sensor_parsers.h) + include/urdf_parser/sensor_parser.h include/urdf_parser/visual_sensor_parsers.h include/urdf_parser/utils.h) target_link_libraries(urdfdom_sensor urdfdom_model) set_target_properties(urdfdom_sensor PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) diff --git a/urdf_parser/include/urdf_parser/utils.h b/urdf_parser/include/urdf_parser/utils.h new file mode 100644 index 00000000..7e170494 --- /dev/null +++ b/urdf_parser/include/urdf_parser/utils.h @@ -0,0 +1,73 @@ +/********************************************************************* +* 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 +#include + +namespace urdf { + +template +T parseAttribute(const char* value) +{ + return boost::lexical_cast(value); +} + +template +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(value); + } + catch (const std::exception &e) + { + throw ParseError(std::string("failed to parse '") + attr + "' attribute: " + e.what()); + } +} + +} + +#endif diff --git a/urdf_parser/src/sensor_parser.cpp b/urdf_parser/src/sensor_parser.cpp index f97b7488..fd3bfc3b 100644 --- a/urdf_parser/src/sensor_parser.cpp +++ b/urdf_parser/src/sensor_parser.cpp @@ -36,11 +36,10 @@ #include "urdf_parser/sensor_parser.h" + #include "urdf_parser/pose.h" #include #include - -#include #include namespace urdf { diff --git a/urdf_parser/src/visual_sensor_parsers.cpp b/urdf_parser/src/visual_sensor_parsers.cpp index 446eae63..242115cd 100644 --- a/urdf_parser/src/visual_sensor_parsers.cpp +++ b/urdf_parser/src/visual_sensor_parsers.cpp @@ -35,12 +35,11 @@ /* Author: John Hsu */ #include "urdf_parser/visual_sensor_parsers.h" +#include "urdf_parser/utils.h" +#include "urdf_parser/pose.h" #include #include - -#include #include -#include "urdf_parser/pose.h" namespace urdf { @@ -51,110 +50,19 @@ SensorBaseSharedPtr CameraParser::parse(TiXmlElement &config) TiXmlElement *image = config.FirstChildElement("image"); if (image) { - const char* width_char = image->Attribute("width"); - if (width_char) - { - try - { - camera->width = boost::lexical_cast(width_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Camera image width [%s] is not a valid int: %s", width_char, e.what()); - return CameraSharedPtr(); - } + try { + camera->width = parseAttribute(*image, "width"); + camera->height = parseAttribute(*image, "height"); + camera->format = parseAttribute(*image, "format"); + camera->hfov = parseAttribute(*image, "hfov"); + camera->near = parseAttribute(*image, "near"); + camera->far = parseAttribute(*image, "far"); } - else + catch (const std::exception &e) { - CONSOLE_BRIDGE_logError("Camera sensor needs an image width attribute"); + CONSOLE_BRIDGE_logError("Camera sensor %s", e.what()); return CameraSharedPtr(); } - - const char* height_char = image->Attribute("height"); - if (height_char) - { - try - { - camera->height = boost::lexical_cast(height_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Camera image height [%s] is not a valid int: %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 = boost::lexical_cast(hfov_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Camera image hfov [%s] is not a valid float: %s", hfov_char, e.what()); - 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 = boost::lexical_cast(near_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Camera image near [%s] is not a valid float: %s", near_char, e.what()); - return CameraSharedPtr(); - } - } - else - { - CONSOLE_BRIDGE_logError("Camera sensor needs an image near attribute"); - return CameraSharedPtr(); - } - - const char* far_char = image->Attribute("far"); - if (far_char) - { - try - { - camera->far = boost::lexical_cast(far_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Camera image far [%s] is not a valid float: %s", far_char, e.what()); - return CameraSharedPtr(); - } - } - else - { - CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute"); - return CameraSharedPtr(); - } - } else { @@ -172,120 +80,32 @@ SensorBaseSharedPtr RayParser::parse(TiXmlElement &config) TiXmlElement *horizontal = config.FirstChildElement("horizontal"); if (horizontal) { - const char* samples_char = horizontal->Attribute("samples"); - if (samples_char) - { - try - { - ray->horizontal_samples = boost::lexical_cast(samples_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what()); - return RaySharedPtr(); - } - } - - const char* resolution_char = horizontal->Attribute("resolution"); - if (resolution_char) - { - try - { - ray->horizontal_resolution = boost::lexical_cast(resolution_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Ray horizontal resolution [%s] is not a valid float: %s", resolution_char, e.what()); - return RaySharedPtr(); - } + try { + ray->horizontal_samples = parseAttribute(*horizontal, "samples"); + ray->horizontal_resolution = parseAttribute(*horizontal, "resolution"); + ray->horizontal_min_angle = parseAttribute(*horizontal, "min_angle"); + ray->horizontal_max_angle = parseAttribute(*horizontal, "max_angle"); } - - const char* min_angle_char = horizontal->Attribute("min_angle"); - if (min_angle_char) - { - try - { - ray->horizontal_min_angle = boost::lexical_cast(min_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Ray horizontal min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); - return RaySharedPtr(); - } - } - - const char* max_angle_char = horizontal->Attribute("max_angle"); - if (max_angle_char) + catch (const std::exception &e) { - try - { - ray->horizontal_max_angle = boost::lexical_cast(max_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Ray horizontal max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); - 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 = boost::lexical_cast(samples_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what()); - return RaySharedPtr(); - } - } - - const char* resolution_char = vertical->Attribute("resolution"); - if (resolution_char) - { - try - { - ray->vertical_resolution = boost::lexical_cast(resolution_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Ray vertical resolution [%s] is not a valid float: %s", resolution_char, e.what()); - return RaySharedPtr(); - } - } - - const char* min_angle_char = vertical->Attribute("min_angle"); - if (min_angle_char) - { - try - { - ray->vertical_min_angle = boost::lexical_cast(min_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Ray vertical min_angle [%s] is not a valid float: %s", min_angle_char, e.what()); - return RaySharedPtr(); - } + try { + ray->vertical_samples = parseAttribute(*vertical, "samples"); + ray->vertical_resolution = parseAttribute(*vertical, "resolution"); + ray->vertical_min_angle = parseAttribute(*vertical, "min_angle"); + ray->vertical_max_angle = parseAttribute(*vertical, "max_angle"); } - - const char* max_angle_char = vertical->Attribute("max_angle"); - if (max_angle_char) + catch (const std::exception &e) { - try - { - ray->vertical_max_angle = boost::lexical_cast(max_angle_char); - } - catch (boost::bad_lexical_cast &e) - { - CONSOLE_BRIDGE_logError("Ray vertical max_angle [%s] is not a valid float: %s", max_angle_char, e.what()); - return RaySharedPtr(); - } + CONSOLE_BRIDGE_logError("Ray horizontal: %s", e.what()); + return RaySharedPtr(); } } return ray;