From c43efd87771b2e56d5d38e78115377d8bf375025 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 14 Mar 2016 23:22:40 +0100 Subject: [PATCH] refactored sensor parsing - parse sensor-specific content with a SensorParser (extension point) - implemented parseCamera() and parseRay() as SensorParsers (visual_sensor_parsers.*) - added unittest test_sensor_parsing --- urdf_parser/CMakeLists.txt | 11 +- .../include/urdf_parser/sensor_parser.h | 72 +++++++ .../{sensor.h => visual_sensor_parsers.h} | 19 +- urdf_parser/src/model.cpp | 2 +- urdf_parser/src/sensor_parser.cpp | 155 ++++++++++++++ ...f_sensor.cpp => visual_sensor_parsers.cpp} | 190 ++++++------------ urdf_parser/test/basic.urdf | 10 + urdf_parser/test/urdf_unit_test.cpp | 59 ++++++ 8 files changed, 378 insertions(+), 140 deletions(-) create mode 100644 urdf_parser/include/urdf_parser/sensor_parser.h rename urdf_parser/include/urdf_parser/{sensor.h => visual_sensor_parsers.h} (81%) create mode 100644 urdf_parser/src/sensor_parser.cpp rename urdf_parser/src/{urdf_sensor.cpp => visual_sensor_parsers.cpp} (63%) diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index a8e57434..baf284cf 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -8,7 +8,9 @@ add_library(urdfdom_model SHARED target_link_libraries(urdfdom_model ${TinyXML_LIBRARIES} ${console_bridge_LIBRARIES} ${Boost_LIBRARIES}) set_target_properties(urdfdom_model PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) -add_library(urdfdom_sensor SHARED src/urdf_sensor.cpp) +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) target_link_libraries(urdfdom_sensor urdfdom_model) set_target_properties(urdfdom_sensor PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) @@ -38,8 +40,11 @@ INSTALL(TARGETS check_urdf urdf_to_graphiz urdf_mem_test DESTINATION ${CMAKE_INSTALL_BINDIR}) INSTALL(TARGETS urdfdom_sensor DESTINATION ${CMAKE_INSTALL_LIBDIR}) INSTALL(TARGETS urdfdom_model_state DESTINATION ${CMAKE_INSTALL_LIBDIR}) -INSTALL(FILES include/urdf_parser/exportdecl.h +INSTALL(FILES + include/urdf_parser/exportdecl.h include/urdf_parser/urdf_parser.h + include/urdf_parser/sensor_parser.h + include/urdf_parser/visual_sensor_parsers.h include/urdf_parser/link.h include/urdf_parser/pose.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/urdf_parser) @@ -47,7 +52,7 @@ INSTALL(FILES include/urdf_parser/exportdecl.h # unit tests add_executable(urdf_unit_test test/urdf_unit_test.cpp) -target_link_libraries(urdf_unit_test urdfdom_model ${Boost_LIBRARIES}) +target_link_libraries(urdf_unit_test urdfdom_model urdfdom_sensor ${Boost_LIBRARIES}) add_test(NAME urdf_unit_test WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/test COMMAND urdf_unit_test) diff --git a/urdf_parser/include/urdf_parser/sensor_parser.h b/urdf_parser/include/urdf_parser/sensor_parser.h new file mode 100644 index 00000000..2f2c7b7a --- /dev/null +++ b/urdf_parser/include/urdf_parser/sensor_parser.h @@ -0,0 +1,72 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* 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 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: Robert Haschke */ + +#ifndef URDF_PARSER_URDF_SENSOR_PARSER_H +#define URDF_PARSER_URDF_SENSOR_PARSER_H + +#include +#include +#include +#include + +#include "exportdecl.h" + +namespace urdf { + + URDF_TYPEDEF_CLASS_POINTER(SensorParser); + class URDFDOM_DLLAPI SensorParser { + public: + virtual ~SensorParser() {} + virtual SensorBaseSharedPtr parse(TiXmlElement &sensor_element) = 0; + }; + + typedef std::map SensorMap; + typedef std::map SensorParserMap; + + /** parse tags in URDF document for which a parser exists in SensorParserMap */ + URDFDOM_DLLAPI SensorMap parseSensors(TiXmlDocument &urdf, const SensorParserMap &parsers); + + /** convienency function to fetch a sensor with given name and type from the map */ + template + URDFDOM_DLLAPI boost::shared_ptr getSensor(const std::string &name, SensorMap &sensors) { + SensorMap::iterator s = sensors.find(name); + if (s == sensors.end()) return boost::shared_ptr(); + else return boost::dynamic_pointer_cast(s->second->sensor_); + } + +} + +#endif diff --git a/urdf_parser/include/urdf_parser/sensor.h b/urdf_parser/include/urdf_parser/visual_sensor_parsers.h similarity index 81% rename from urdf_parser/include/urdf_parser/sensor.h rename to urdf_parser/include/urdf_parser/visual_sensor_parsers.h index 80a5e14a..69ff78ad 100644 --- a/urdf_parser/include/urdf_parser/sensor.h +++ b/urdf_parser/include/urdf_parser/visual_sensor_parsers.h @@ -32,15 +32,24 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef URDF_PARSER_SENSOR_H -#define URDF_PARSER_SENSOR_H +/* Author: Robert Haschke */ -#include -#include +#ifndef URDF_PARSER_VISUAL_SENSOR_PARSERS_H +#define URDF_PARSER_VISUAL_SENSOR_PARSERS_H + +#include "sensor_parser.h" namespace urdf { -bool parseSensor(Sensor &sensor, TiXmlElement* config); + class URDFDOM_DLLAPI CameraParser : public SensorParser { + public: + SensorBaseSharedPtr parse(TiXmlElement &sensor_element); + }; + + class URDFDOM_DLLAPI RayParser : public SensorParser { + public: + SensorBaseSharedPtr parse(TiXmlElement &sensor_element); + }; } diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index 79c0d917..0dbf3102 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -261,7 +261,7 @@ TiXmlDocument* exportURDF(const ModelInterface &model) CONSOLE_BRIDGE_logDebug("urdfdom: exporting link [%s]\n",l->second->name.c_str()); exportLink(*(l->second), robot); } - + for (std::map::const_iterator j=model.joints_.begin(); j!=model.joints_.end(); j++) { CONSOLE_BRIDGE_logDebug("urdfdom: exporting joint [%s]\n",j->second->name.c_str()); diff --git a/urdf_parser/src/sensor_parser.cpp b/urdf_parser/src/sensor_parser.cpp new file mode 100644 index 00000000..f97b7488 --- /dev/null +++ b/urdf_parser/src/sensor_parser.cpp @@ -0,0 +1,155 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* 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 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: John Hsu */ + + +#include "urdf_parser/sensor_parser.h" +#include "urdf_parser/pose.h" +#include +#include + +#include +#include + +namespace urdf { + +SensorBaseSharedPtr parseSensorBase(TiXmlElement *sensor_xml, const SensorParserMap &parsers) +{ + SensorBaseSharedPtr sensor_; + + + // find first child element that is not or + const char* sensor_type; + TiXmlElement *sensor_base_xml = sensor_xml->FirstChildElement(); + while (sensor_base_xml) { + sensor_type = sensor_base_xml->Value(); + if (strcmp(sensor_type, "parent") && strcmp(sensor_type, "origin")) + break; + sensor_base_xml = sensor_base_xml->NextSiblingElement(); + } + + if (sensor_base_xml) + { + SensorParserMap::const_iterator parser = parsers.find(sensor_type); + if (parser != parsers.end() && parser->second) + { + return parser->second->parse(*sensor_base_xml); + } + else + { + CONSOLE_BRIDGE_logDebug("Unknown sensor type %s", sensor_type); + } + } + else + { + CONSOLE_BRIDGE_logError("no child element defining the sensor"); + } + + return sensor_; +} + + +bool parseSensor(Sensor &sensor, TiXmlElement* config, const SensorParserMap &parsers) +{ + sensor.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + CONSOLE_BRIDGE_logError("No name given for the sensor."); + return false; + } + sensor.name_ = std::string(name_char); + + // parse parent link name + TiXmlElement *parent_xml = config->FirstChildElement("parent"); + const char *parent_link_name_char = parent_xml ? parent_xml->Attribute("link") : NULL; + if (!parent_link_name_char) + { + CONSOLE_BRIDGE_logError("No parent link name given for the sensor."); + return false; + } + sensor.parent_link_ = std::string(parent_link_name_char); + + // parse origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) + { + if (!parsePose(sensor.origin_, o)) + return false; + } + + // parse sensor + sensor.sensor_ = parseSensorBase(config, parsers); + return true; +} + +URDFDOM_DLLAPI +SensorMap parseSensors(TiXmlDocument &urdf_xml, const SensorParserMap &parsers) +{ + TiXmlElement *robot_xml = urdf_xml.FirstChildElement("robot"); + if (!robot_xml) { + CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the URDF"); + } + + SensorMap results; + // Get all sensor elements + for (TiXmlElement* sensor_xml = robot_xml->FirstChildElement("sensor"); + sensor_xml; sensor_xml = sensor_xml->NextSiblingElement("sensor")) + { + SensorSharedPtr sensor; + sensor.reset(new Sensor); + + if (parseSensor(*sensor, sensor_xml, parsers)) + { + if (results.find(sensor->name_) != results.end()) + { + CONSOLE_BRIDGE_logWarn("Sensor '%s' is not unique. Ignoring consecutive ones.", sensor->name_.c_str()); + } + else + { + results.insert(make_pair(sensor->name_, sensor)); + CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new sensor '%s'", sensor->name_.c_str()); + } + } + else + { + CONSOLE_BRIDGE_logError("failed to parse sensor element"); + } + } + return results; +} + +} diff --git a/urdf_parser/src/urdf_sensor.cpp b/urdf_parser/src/visual_sensor_parsers.cpp similarity index 63% rename from urdf_parser/src/urdf_sensor.cpp rename to urdf_parser/src/visual_sensor_parsers.cpp index 805ebce2..446eae63 100644 --- a/urdf_parser/src/urdf_sensor.cpp +++ b/urdf_parser/src/visual_sensor_parsers.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2008, Willow Garage, Inc. * 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 @@ -17,7 +17,7 @@ * * Neither the name of the 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 @@ -34,24 +34,21 @@ /* Author: John Hsu */ +#include "urdf_parser/visual_sensor_parsers.h" +#include +#include -#include -#include -#include #include -#include -#include #include #include "urdf_parser/pose.h" -namespace urdf{ +namespace urdf { -bool parseCamera(Camera &camera, TiXmlElement* config) +SensorBaseSharedPtr CameraParser::parse(TiXmlElement &config) { - camera.clear(); - camera.type = VisualSensor::CAMERA; + CameraSharedPtr camera(new Camera()); - TiXmlElement *image = config->FirstChildElement("image"); + TiXmlElement *image = config.FirstChildElement("image"); if (image) { const char* width_char = image->Attribute("width"); @@ -59,18 +56,18 @@ bool parseCamera(Camera &camera, TiXmlElement* config) { try { - camera.width = boost::lexical_cast(width_char); + 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 false; + return CameraSharedPtr(); } } else { CONSOLE_BRIDGE_logError("Camera sensor needs an image width attribute"); - return false; + return CameraSharedPtr(); } const char* height_char = image->Attribute("height"); @@ -78,46 +75,46 @@ bool parseCamera(Camera &camera, TiXmlElement* config) { try { - camera.height = boost::lexical_cast(height_char); + 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 false; + return CameraSharedPtr(); } } else { CONSOLE_BRIDGE_logError("Camera sensor needs an image height attribute"); - return false; + return CameraSharedPtr(); } const char* format_char = image->Attribute("format"); if (format_char) - camera.format = std::string(format_char); + camera->format = std::string(format_char); else { CONSOLE_BRIDGE_logError("Camera sensor needs an image format attribute"); - return false; - } + return CameraSharedPtr(); + } const char* hfov_char = image->Attribute("hfov"); if (hfov_char) { try { - camera.hfov = boost::lexical_cast(hfov_char); + 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 false; + return CameraSharedPtr(); } } else { CONSOLE_BRIDGE_logError("Camera sensor needs an image hfov attribute"); - return false; + return CameraSharedPtr(); } const char* near_char = image->Attribute("near"); @@ -125,18 +122,18 @@ bool parseCamera(Camera &camera, TiXmlElement* config) { try { - camera.near = boost::lexical_cast(near_char); + 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 false; + return CameraSharedPtr(); } } else { CONSOLE_BRIDGE_logError("Camera sensor needs an image near attribute"); - return false; + return CameraSharedPtr(); } const char* far_char = image->Attribute("far"); @@ -144,35 +141,35 @@ bool parseCamera(Camera &camera, TiXmlElement* config) { try { - camera.far = boost::lexical_cast(far_char); + 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 false; + return CameraSharedPtr(); } } else { CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute"); - return false; + return CameraSharedPtr(); } - + } else { CONSOLE_BRIDGE_logError("Camera sensor has no element"); - return false; + return CameraSharedPtr(); } - return true; + return camera; } -bool parseRay(Ray &ray, TiXmlElement* config) + +SensorBaseSharedPtr RayParser::parse(TiXmlElement &config) { - ray.clear(); - ray.type = VisualSensor::RAY; + RaySharedPtr ray (new Ray()); - TiXmlElement *horizontal = config->FirstChildElement("horizontal"); + TiXmlElement *horizontal = config.FirstChildElement("horizontal"); if (horizontal) { const char* samples_char = horizontal->Attribute("samples"); @@ -180,12 +177,12 @@ bool parseRay(Ray &ray, TiXmlElement* config) { try { - ray.horizontal_samples = boost::lexical_cast(samples_char); + 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 false; + return RaySharedPtr(); } } @@ -194,26 +191,26 @@ bool parseRay(Ray &ray, TiXmlElement* config) { try { - ray.horizontal_resolution = boost::lexical_cast(resolution_char); + 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 false; + return RaySharedPtr(); } - } - + } + const char* min_angle_char = horizontal->Attribute("min_angle"); if (min_angle_char) { try { - ray.horizontal_min_angle = boost::lexical_cast(min_angle_char); + 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 false; + return RaySharedPtr(); } } @@ -222,17 +219,17 @@ bool parseRay(Ray &ray, TiXmlElement* config) { try { - ray.horizontal_max_angle = boost::lexical_cast(max_angle_char); + 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 false; + return RaySharedPtr(); } } } - - TiXmlElement *vertical = config->FirstChildElement("vertical"); + + TiXmlElement *vertical = config.FirstChildElement("vertical"); if (vertical) { const char* samples_char = vertical->Attribute("samples"); @@ -240,12 +237,12 @@ bool parseRay(Ray &ray, TiXmlElement* config) { try { - ray.vertical_samples = boost::lexical_cast(samples_char); + 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 false; + return RaySharedPtr(); } } @@ -254,26 +251,26 @@ bool parseRay(Ray &ray, TiXmlElement* config) { try { - ray.vertical_resolution = boost::lexical_cast(resolution_char); + 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 false; + 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); + 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 false; + return RaySharedPtr(); } } @@ -282,85 +279,16 @@ bool parseRay(Ray &ray, TiXmlElement* config) { try { - ray.vertical_max_angle = boost::lexical_cast(max_angle_char); + 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 false; + return RaySharedPtr(); } } } - return true; + return ray; } -SensorBaseSharedPtr parseSensorBase(TiXmlElement *g) -{ - SensorBaseSharedPtr sensor_; - - // get sensor type - TiXmlElement *sensor_xml; - if (g->FirstChildElement("camera")) - { - Camera *camera = new Camera(); - sensor_.reset(camera); - sensor_->sensor_type = SensorBase::VISUAL; - sensor_xml = g->FirstChildElement("camera"); - if (!parseCamera(*camera, sensor_xml)) - sensor_.reset(); - } - else if (g->FirstChildElement("ray")) - { - Ray *ray = new Ray(); - sensor_.reset(ray); - sensor_->sensor_type = SensorBase::VISUAL; - sensor_xml = g->FirstChildElement("ray"); - if (!parseRay(*ray, sensor_xml)) - sensor_.reset(); - } - else - { - CONSOLE_BRIDGE_logError("No know sensor types [camera|ray] defined in block"); - } - - return sensor_; } - -bool parseSensor(Sensor &sensor, TiXmlElement* config) -{ - sensor.clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - CONSOLE_BRIDGE_logError("No name given for the sensor."); - return false; - } - sensor.name = std::string(name_char); - - // parse parent link name - TiXmlElement *parent_xml = config->FirstChildElement("parent"); - const char *parent_link_name_char = parent_xml ? parent_xml->Attribute("link") : NULL; - if (!parent_link_name_char) - { - CONSOLE_BRIDGE_logError("No parent link name given for the sensor."); - return false; - } - sensor.parent_link_name = std::string(parent_link_name_char); - - // parse origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (o) - { - if (!parsePose(sensor.origin, o)) - return false; - } - - // parse sensor - sensor.sensor = parseSensorBase(config); - return true; -} - -} - - diff --git a/urdf_parser/test/basic.urdf b/urdf_parser/test/basic.urdf index dce56198..7dcf76b1 100644 --- a/urdf_parser/test/basic.urdf +++ b/urdf_parser/test/basic.urdf @@ -21,4 +21,14 @@ + + + + + + + + + + diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index ea131439..ea5b0d23 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -1,8 +1,15 @@ #include +#include +#include +#include +#include + #include +#include #include #include #include +#include // the name of our test module #define BOOST_TEST_MODULE URDF_UNIT_TEST @@ -156,3 +163,55 @@ BOOST_AUTO_TEST_CASE(test_only_consider_top_level) BOOST_CHECK(!model->getLink("link2")); BOOST_CHECK(!model->getJoint("joint1")); } + +static boost::shared_ptr loadFromFile(const std::string &path) +{ + std::ifstream stream(path.c_str()); + BOOST_REQUIRE_MESSAGE(stream, "failed to open file"); + + std::string xml_str((std::istreambuf_iterator(stream)), + std::istreambuf_iterator()); + + boost::shared_ptr xml_doc(new TiXmlDocument()); + xml_doc->Parse(xml_str.c_str()); + BOOST_REQUIRE(!xml_doc->Error()); + + return xml_doc; +} + +BOOST_AUTO_TEST_CASE(test_sensor_parsing) +{ + boost::shared_ptr xml_doc = loadFromFile("basic.urdf"); + urdf::SensorParserMap parsers; + parsers.insert(std::make_pair("camera", urdf::SensorParserSharedPtr(new urdf::CameraParser))); + parsers.insert(std::make_pair("ray", urdf::SensorParserSharedPtr(new urdf::RayParser))); + + urdf::SensorMap sensors = urdf::parseSensors(*xml_doc, parsers); + + BOOST_CHECK(!urdf::getSensor("camera1", sensors)); + urdf::CameraSharedPtr camera = urdf::getSensor("camera1", sensors); + BOOST_CHECK(camera); + if (camera) { + BOOST_CHECK_EQUAL(camera->width, 640); + BOOST_CHECK_EQUAL(camera->height, 480); + BOOST_CHECK_EQUAL(camera->format, "RGB8"); + BOOST_CHECK_EQUAL(camera->hfov, 1.5708); + BOOST_CHECK_EQUAL(camera->near, 0.01); + BOOST_CHECK_EQUAL(camera->far, 50.0); + } + + BOOST_CHECK(!urdf::getSensor("ray1", sensors)); + urdf::RaySharedPtr ray = urdf::getSensor("ray1", sensors); + BOOST_CHECK(ray); + if (ray) { + BOOST_CHECK_EQUAL(ray->horizontal_samples, 100); + BOOST_CHECK_EQUAL(ray->horizontal_resolution, 1); + BOOST_CHECK_EQUAL(ray->horizontal_min_angle, -1.5708); + BOOST_CHECK_EQUAL(ray->horizontal_max_angle, +1.5708); + + BOOST_CHECK_EQUAL(ray->vertical_samples, 1); + BOOST_CHECK_EQUAL(ray->vertical_resolution, 1); + BOOST_CHECK_EQUAL(ray->vertical_min_angle, 0); + BOOST_CHECK_EQUAL(ray->vertical_max_angle, 0); + } +}