diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index 9ce270bf..e2071e2d 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -47,7 +47,7 @@ include_directories( link_directories(${Boost_LIBRARY_DIRS} ${catkin_LIBRARY_DIRS}) -add_library(${PROJECT_NAME} src/model.cpp src/rosconsole_bridge.cpp) +add_library(${PROJECT_NAME} src/model.cpp src/sensor.cpp src/rosconsole_bridge.cpp) target_link_libraries(${PROJECT_NAME} ${TinyXML_LIBRARIES} ${TinyXML2_LIBRARIES} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES}) if(WIN32) diff --git a/urdf/include/urdf/sensor.h b/urdf/include/urdf/sensor.h new file mode 100644 index 00000000..04977e09 --- /dev/null +++ b/urdf/include/urdf/sensor.h @@ -0,0 +1,74 @@ +/********************************************************************* +* 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 authors 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_SENSOR_H +#define URDF_SENSOR_H + +#include +#include +#include +#include + +namespace urdf { + +// Maintain class loader together with created parser instances +class ManagedSensorParserMap : public SensorParserMap { +public: + std::unique_ptr> loader; + ManagedSensorParserMap(); + ~ManagedSensorParserMap(); + ManagedSensorParserMap(const ManagedSensorParserMap&) = delete; + ManagedSensorParserMap(ManagedSensorParserMap &&) = default; + ManagedSensorParserMap &operator=(ManagedSensorParserMap &&) = default; +}; + +/** Retrieve sensor parsers available through the plugin-lib mechanism + whose name matches any of the names listed in allowed. + If allowed is empty (the default), all parsers will be returned. +*/ +urdf::ManagedSensorParserMap getSensorParsers(const std::vector &allowed = std::vector()); + +/** Conveniency method returning the SensorParserMap for the given sensor name */ +urdf::ManagedSensorParserMap getSensorParser(const std::string &name); + +/** parse tags in URDF document */ +SensorMap parseSensors(const std::string &xml, const urdf::SensorParserMap &parsers); +SensorMap parseSensorsFromParam(const std::string ¶m, const urdf::SensorParserMap &parsers); +SensorMap parseSensorsFromFile(const std::string &filename, const urdf::SensorParserMap &parsers); + +} + +#endif diff --git a/urdf/package.xml b/urdf/package.xml index 5bcc695d..51776b7a 100644 --- a/urdf/package.xml +++ b/urdf/package.xml @@ -21,27 +21,15 @@ catkin - liburdfdom-dev - liburdfdom-headers-dev - rosconsole_bridge - roscpp + urdfdom_headers + urdfdom + rosconsole_bridge + roscpp urdf_parser_plugin - pluginlib + pluginlib cmake_modules - tinyxml - tinyxml2 - - liburdfdom-dev - rosconsole_bridge - roscpp - pluginlib - tinyxml - tinyxml2 - - tinyxml - tinyxml2 - liburdfdom-headers-dev + tinyxml + tinyxml2 rostest - diff --git a/urdf/src/sensor.cpp b/urdf/src/sensor.cpp new file mode 100644 index 00000000..ca6101b1 --- /dev/null +++ b/urdf/src/sensor.cpp @@ -0,0 +1,136 @@ +/********************************************************************* +* 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 authors 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 */ + +#include "urdf/sensor.h" + +#include +#include +#include + +namespace urdf { + +ManagedSensorParserMap::ManagedSensorParserMap() + : loader(new pluginlib::ClassLoader("urdf", "urdf::SensorParser")) +{} + +ManagedSensorParserMap::~ManagedSensorParserMap() { + clear(); // first destroy parser instances + loader.reset(); // and subsequently the loader +} + +SensorMap parseSensorsFromFile(const std::string &filename, const SensorParserMap &parsers) +{ + SensorMap result; + std::ifstream stream(filename.c_str()); + if (!stream.is_open()) + { + throw std::runtime_error("Could not open file [" + filename + "] for parsing."); + } + + std::string xml_string((std::istreambuf_iterator(stream)), + std::istreambuf_iterator()); + return parseSensors(xml_string, parsers); +} + + +SensorMap parseSensorsFromParam(const std::string ¶m, const SensorParserMap &parsers) +{ + ros::NodeHandle nh; + std::string xml_string; + + // gets the location of the robot description on the parameter server + std::string full_param; + if (!nh.searchParam(param, full_param)){ + throw std::runtime_error("Could not find parameter " + param + " on parameter server"); + } + + // read the robot description from the parameter server + if (!nh.getParam(full_param, xml_string)){ + throw std::runtime_error("Could not read parameter " + param + " on parameter server"); + } + return parseSensors(xml_string, parsers); +} + + +SensorMap parseSensors(const std::string &xml_string, const SensorParserMap &parsers) +{ + TiXmlDocument xml_doc; + xml_doc.Parse(xml_string.c_str()); + if (xml_doc.Error()) + throw std::runtime_error(std::string("Could not parse the xml document: ") + xml_doc.ErrorDesc()); + return parseSensors(xml_doc, parsers); +} + +ManagedSensorParserMap getSensorParsers(const std::vector &allowed) +{ + pluginlib::ClassLoader loader("urdf", "urdf::SensorParser"); + ManagedSensorParserMap parserMap; + try + { + const std::vector &classes = parserMap.loader->getDeclaredClasses(); + for (std::size_t i = 0 ; i < classes.size() ; ++i) + { + // skip this class if not listed in allowed + if (!allowed.empty() && std::find(allowed.begin(), allowed.end(), classes[i]) == allowed.end()) + continue; + + urdf::SensorParserSharedPtr parser; + try { + parser = parserMap.loader->createUniqueInstance(classes[i]); + } catch(const pluginlib::PluginlibException& ex) { + ROS_ERROR_STREAM("Failed to create sensor parser: " << classes[i] << "\n" << ex.what()); + } + parserMap.insert(std::make_pair(classes[i], parser)); + ROS_DEBUG_STREAM("added sensor parser: " << classes[i]); + } + if (parserMap.empty()) + ROS_WARN_STREAM("No sensor parsers found"); + } + catch(const pluginlib::PluginlibException& ex) + { + ROS_ERROR_STREAM("Exception while creating sensor plugin loader " << ex.what()); + } + return parserMap; +} + +ManagedSensorParserMap getSensorParser(const std::string &name) +{ + std::vector allowed; + allowed.push_back(name); + return getSensorParsers(allowed); +} + +} // namespace