From 30f4821fefff9de2c7cfb99025aad1799c403e31 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 10 Mar 2016 13:41:19 +0100 Subject: [PATCH 1/8] added sensor parsing --- urdf/CMakeLists.txt | 2 +- urdf/include/urdf/sensor.h | 57 +++++++++++ urdf/src/sensor.cpp | 187 +++++++++++++++++++++++++++++++++++++ 3 files changed, 245 insertions(+), 1 deletion(-) create mode 100644 urdf/include/urdf/sensor.h create mode 100644 urdf/src/sensor.cpp 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..9047f9bb --- /dev/null +++ b/urdf/include/urdf/sensor.h @@ -0,0 +1,57 @@ +/********************************************************************* +* 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 + +namespace urdf { + +/** retrieve all sensor parsers available in the system + through the plugin-lib mechanism */ +const urdf::SensorParserMap &getDefaultSensorParserMap(); + +/** 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/src/sensor.cpp b/urdf/src/sensor.cpp new file mode 100644 index 00000000..f33d1856 --- /dev/null +++ b/urdf/src/sensor.cpp @@ -0,0 +1,187 @@ +/********************************************************************* +* 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 +#include + +namespace urdf { + +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); +} + + +/** retrieve list of sensor tags that are handled by given parser */ +static std::set +getSensorTags(pluginlib::ClassLoader &loader, + const std::string &class_id) +{ + const std::string &manifest = loader.getPluginManifestPath(class_id); + + TiXmlDocument doc; + doc.LoadFile(manifest); + TiXmlElement *library = doc.RootElement(); + if (!library) + throw std::runtime_error("Skipping manifest '" + manifest + "' which failed to parse"); + + if (library->ValueStr() != "library" && + library->ValueStr() != "class_libraries") + throw std::runtime_error("Expected \"library\" or \"class_libraries\" as the root tag." + "Found: " + library->ValueStr()); + + if (library->ValueStr() == "class_libraries") + library = library->FirstChildElement("library"); + + std::set results; + for (; library; library = library->FirstChildElement("library")) + { + for (TiXmlElement* class_element = library->FirstChildElement("class"); + class_element; class_element = class_element->NextSiblingElement( "class" )) + { + // TODO: filter by class_id + ROS_DEBUG_STREAM("sensor parser: " << class_id); + TiXmlElement* tags = class_element->FirstChildElement("tags"); + for (TiXmlElement* tag = tags ? tags->FirstChildElement() : NULL; + tag; tag = tag->NextSiblingElement()) + { + ROS_DEBUG_STREAM(" sensor tag: " << tag->Value()); + results.insert(tag->Value()); + } + } + } + if (results.empty()) + throw std::runtime_error("plugin manifest misses valid sensor tags"); + return results; +} + +const SensorParserMap& getDefaultSensorParserMap() +{ + static boost::mutex PARSER_PLUGIN_LOCK; + static boost::shared_ptr > PARSER_PLUGIN_LOADER; + static SensorParserMap defaultParserMap; + + boost::mutex::scoped_lock _(PARSER_PLUGIN_LOCK); + try + { + if (!PARSER_PLUGIN_LOADER) { + PARSER_PLUGIN_LOADER.reset(new pluginlib::ClassLoader("urdf", "urdf::SensorParser")); + + const std::vector &classes = PARSER_PLUGIN_LOADER->getDeclaredClasses(); + for (std::size_t i = 0 ; i < classes.size() ; ++i) + { + std::set sensor_tags; + try { + sensor_tags = getSensorTags(*PARSER_PLUGIN_LOADER, classes[i]); + } catch (const std::runtime_error &e) { + ROS_ERROR_STREAM(e.what()); + continue; + } + + urdf::SensorParserSharedPtr parser; + try { + parser = PARSER_PLUGIN_LOADER->createInstance(classes[i]); + } catch(const pluginlib::PluginlibException& ex) { + ROS_ERROR_STREAM("Failed to create sensor parser: " << classes[i] << "\n" << ex.what()); + } + + for (std::set::const_iterator + it = sensor_tags.begin(), end=sensor_tags.end(); it != end; ++it) + { + if (defaultParserMap.find(*it) == defaultParserMap.end()) + { + defaultParserMap.insert(std::make_pair(*it, parser)); + } + else + { + ROS_WARN("ambiguous sensor parser for sensor %s", it->c_str()); + } + } + } + if (defaultParserMap.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 defaultParserMap; +} + +} // namespace From 5f1a30ba3407cfaec4a342b03183a2e46287a7dd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 17 Mar 2016 17:14:14 +0100 Subject: [PATCH 2/8] simplified code: use name attribute as identifier for xml element --- urdf/src/sensor.cpp | 67 ++------------------------------------------- 1 file changed, 2 insertions(+), 65 deletions(-) diff --git a/urdf/src/sensor.cpp b/urdf/src/sensor.cpp index f33d1856..3b7897d7 100644 --- a/urdf/src/sensor.cpp +++ b/urdf/src/sensor.cpp @@ -86,50 +86,6 @@ SensorMap parseSensors(const std::string &xml_string, const SensorParserMap &par return parseSensors(xml_doc, parsers); } - -/** retrieve list of sensor tags that are handled by given parser */ -static std::set -getSensorTags(pluginlib::ClassLoader &loader, - const std::string &class_id) -{ - const std::string &manifest = loader.getPluginManifestPath(class_id); - - TiXmlDocument doc; - doc.LoadFile(manifest); - TiXmlElement *library = doc.RootElement(); - if (!library) - throw std::runtime_error("Skipping manifest '" + manifest + "' which failed to parse"); - - if (library->ValueStr() != "library" && - library->ValueStr() != "class_libraries") - throw std::runtime_error("Expected \"library\" or \"class_libraries\" as the root tag." - "Found: " + library->ValueStr()); - - if (library->ValueStr() == "class_libraries") - library = library->FirstChildElement("library"); - - std::set results; - for (; library; library = library->FirstChildElement("library")) - { - for (TiXmlElement* class_element = library->FirstChildElement("class"); - class_element; class_element = class_element->NextSiblingElement( "class" )) - { - // TODO: filter by class_id - ROS_DEBUG_STREAM("sensor parser: " << class_id); - TiXmlElement* tags = class_element->FirstChildElement("tags"); - for (TiXmlElement* tag = tags ? tags->FirstChildElement() : NULL; - tag; tag = tag->NextSiblingElement()) - { - ROS_DEBUG_STREAM(" sensor tag: " << tag->Value()); - results.insert(tag->Value()); - } - } - } - if (results.empty()) - throw std::runtime_error("plugin manifest misses valid sensor tags"); - return results; -} - const SensorParserMap& getDefaultSensorParserMap() { static boost::mutex PARSER_PLUGIN_LOCK; @@ -145,33 +101,14 @@ const SensorParserMap& getDefaultSensorParserMap() const std::vector &classes = PARSER_PLUGIN_LOADER->getDeclaredClasses(); for (std::size_t i = 0 ; i < classes.size() ; ++i) { - std::set sensor_tags; - try { - sensor_tags = getSensorTags(*PARSER_PLUGIN_LOADER, classes[i]); - } catch (const std::runtime_error &e) { - ROS_ERROR_STREAM(e.what()); - continue; - } - urdf::SensorParserSharedPtr parser; try { parser = PARSER_PLUGIN_LOADER->createInstance(classes[i]); } catch(const pluginlib::PluginlibException& ex) { ROS_ERROR_STREAM("Failed to create sensor parser: " << classes[i] << "\n" << ex.what()); } - - for (std::set::const_iterator - it = sensor_tags.begin(), end=sensor_tags.end(); it != end; ++it) - { - if (defaultParserMap.find(*it) == defaultParserMap.end()) - { - defaultParserMap.insert(std::make_pair(*it, parser)); - } - else - { - ROS_WARN("ambiguous sensor parser for sensor %s", it->c_str()); - } - } + defaultParserMap.insert(std::make_pair(classes[i], parser)); + ROS_DEBUG_STREAM("added sensor parser: " << classes[i]); } if (defaultParserMap.empty()) ROS_WARN_STREAM("No sensor parsers found"); From 0fa27d7d5d7921492d0dad365ebcf0ee631769f1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 18 Mar 2016 15:58:51 +0100 Subject: [PATCH 3/8] return a filtered list of parsers only --- urdf/include/urdf/sensor.h | 11 ++++++++--- urdf/src/sensor.cpp | 32 ++++++++++++++++++++++---------- 2 files changed, 30 insertions(+), 13 deletions(-) diff --git a/urdf/include/urdf/sensor.h b/urdf/include/urdf/sensor.h index 9047f9bb..ed8fb904 100644 --- a/urdf/include/urdf/sensor.h +++ b/urdf/include/urdf/sensor.h @@ -43,9 +43,14 @@ namespace urdf { -/** retrieve all sensor parsers available in the system - through the plugin-lib mechanism */ -const urdf::SensorParserMap &getDefaultSensorParserMap(); +/** 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::SensorParserMap getSensorParsers(const std::vector &allowed = std::vector()); + +/** Conveniency method returning the SensorParserMap for the given sensor name */ +urdf::SensorParserMap getSensorParser(const std::string &name); /** parse tags in URDF document */ SensorMap parseSensors(const std::string &xml, const urdf::SensorParserMap &parsers); diff --git a/urdf/src/sensor.cpp b/urdf/src/sensor.cpp index 3b7897d7..08a9a1e3 100644 --- a/urdf/src/sensor.cpp +++ b/urdf/src/sensor.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include namespace urdf { @@ -86,7 +87,7 @@ SensorMap parseSensors(const std::string &xml_string, const SensorParserMap &par return parseSensors(xml_doc, parsers); } -const SensorParserMap& getDefaultSensorParserMap() +SensorParserMap getSensorParsers(const std::vector &allowed) { static boost::mutex PARSER_PLUGIN_LOCK; static boost::shared_ptr > PARSER_PLUGIN_LOADER; @@ -101,14 +102,18 @@ const SensorParserMap& getDefaultSensorParserMap() const std::vector &classes = PARSER_PLUGIN_LOADER->getDeclaredClasses(); for (std::size_t i = 0 ; i < classes.size() ; ++i) { - urdf::SensorParserSharedPtr parser; - try { - parser = PARSER_PLUGIN_LOADER->createInstance(classes[i]); - } catch(const pluginlib::PluginlibException& ex) { - ROS_ERROR_STREAM("Failed to create sensor parser: " << classes[i] << "\n" << ex.what()); - } - defaultParserMap.insert(std::make_pair(classes[i], parser)); - ROS_DEBUG_STREAM("added sensor parser: " << classes[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 = PARSER_PLUGIN_LOADER->createInstance(classes[i]); + } catch(const pluginlib::PluginlibException& ex) { + ROS_ERROR_STREAM("Failed to create sensor parser: " << classes[i] << "\n" << ex.what()); + } + defaultParserMap.insert(std::make_pair(classes[i], parser)); + ROS_DEBUG_STREAM("added sensor parser: " << classes[i]); } if (defaultParserMap.empty()) ROS_WARN_STREAM("No sensor parsers found"); @@ -116,9 +121,16 @@ const SensorParserMap& getDefaultSensorParserMap() } catch(const pluginlib::PluginlibException& ex) { - ROS_ERROR_STREAM("Exception while creating sensor plugin loader " << ex.what()); + ROS_ERROR_STREAM("Exception while creating sensor plugin loader " << ex.what()); } return defaultParserMap; } +SensorParserMap getSensorParser(const std::string &name) +{ + std::vector allowed; + allowed.push_back(name); + return getSensorParsers(allowed); +} + } // namespace From c92b3a2931a80a09f2b61661a884fd380de4d60f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 3 Apr 2016 10:48:32 +0200 Subject: [PATCH 4/8] don't cache defaultParserMap: allowed might change between calls --- urdf/src/sensor.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/urdf/src/sensor.cpp b/urdf/src/sensor.cpp index 08a9a1e3..a7819b71 100644 --- a/urdf/src/sensor.cpp +++ b/urdf/src/sensor.cpp @@ -91,12 +91,12 @@ SensorParserMap getSensorParsers(const std::vector &allowed) { static boost::mutex PARSER_PLUGIN_LOCK; static boost::shared_ptr > PARSER_PLUGIN_LOADER; - static SensorParserMap defaultParserMap; - boost::mutex::scoped_lock _(PARSER_PLUGIN_LOCK); + + SensorParserMap parserMap; try { - if (!PARSER_PLUGIN_LOADER) { + if (!PARSER_PLUGIN_LOADER) PARSER_PLUGIN_LOADER.reset(new pluginlib::ClassLoader("urdf", "urdf::SensorParser")); const std::vector &classes = PARSER_PLUGIN_LOADER->getDeclaredClasses(); @@ -112,18 +112,17 @@ SensorParserMap getSensorParsers(const std::vector &allowed) } catch(const pluginlib::PluginlibException& ex) { ROS_ERROR_STREAM("Failed to create sensor parser: " << classes[i] << "\n" << ex.what()); } - defaultParserMap.insert(std::make_pair(classes[i], parser)); + parserMap.insert(std::make_pair(classes[i], parser)); ROS_DEBUG_STREAM("added sensor parser: " << classes[i]); } - if (defaultParserMap.empty()) + 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 defaultParserMap; + return parserMap; } SensorParserMap getSensorParser(const std::string &name) From e022abcc4833763b0c2a9e7e8de8e2aa53869166 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 3 Apr 2016 10:57:31 +0200 Subject: [PATCH 5/8] don't use a static ClassLoader instance - crashes on exit: https://github.com/ros/class_loader/issues/33 - on-demand-unloading works with https://github.com/ros/class_loader/pull/34 --- urdf/src/sensor.cpp | 42 ++++++++++++++++++------------------------ 1 file changed, 18 insertions(+), 24 deletions(-) diff --git a/urdf/src/sensor.cpp b/urdf/src/sensor.cpp index a7819b71..45b6f461 100644 --- a/urdf/src/sensor.cpp +++ b/urdf/src/sensor.cpp @@ -89,34 +89,28 @@ SensorMap parseSensors(const std::string &xml_string, const SensorParserMap &par SensorParserMap getSensorParsers(const std::vector &allowed) { - static boost::mutex PARSER_PLUGIN_LOCK; - static boost::shared_ptr > PARSER_PLUGIN_LOADER; - boost::mutex::scoped_lock _(PARSER_PLUGIN_LOCK); - + pluginlib::ClassLoader loader("urdf", "urdf::SensorParser"); SensorParserMap parserMap; try { - if (!PARSER_PLUGIN_LOADER) - PARSER_PLUGIN_LOADER.reset(new pluginlib::ClassLoader("urdf", "urdf::SensorParser")); - - const std::vector &classes = PARSER_PLUGIN_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 = PARSER_PLUGIN_LOADER->createInstance(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]); + const std::vector &classes = 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 = loader.createInstance(classes[i]); + } catch(const pluginlib::PluginlibException& ex) { + ROS_ERROR_STREAM("Failed to create sensor parser: " << classes[i] << "\n" << ex.what()); } - if (parserMap.empty()) - ROS_WARN_STREAM("No sensor parsers found"); + 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) { From d87677fee4bbbaa0e571d28fb3da6a5a62a5f122 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 7 Apr 2016 11:03:10 +0200 Subject: [PATCH 6/8] require version >= 1.1 of urdfdom --- urdf/package.xml | 26 +++++++------------------- 1 file changed, 7 insertions(+), 19 deletions(-) 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 - From dab0a4e9d627e70b9a7dc053bc087693bb1eef82 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 21 Sep 2022 10:14:40 +0200 Subject: [PATCH 7/8] ManagedSensorParserMap Destroy class loader only after instances of parsers. --- urdf/include/urdf/sensor.h | 16 ++++++++++++++-- urdf/src/sensor.cpp | 20 ++++++++++++++------ 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/urdf/include/urdf/sensor.h b/urdf/include/urdf/sensor.h index ed8fb904..04977e09 100644 --- a/urdf/include/urdf/sensor.h +++ b/urdf/include/urdf/sensor.h @@ -40,17 +40,29 @@ #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::SensorParserMap getSensorParsers(const std::vector &allowed = std::vector()); +urdf::ManagedSensorParserMap getSensorParsers(const std::vector &allowed = std::vector()); /** Conveniency method returning the SensorParserMap for the given sensor name */ -urdf::SensorParserMap getSensorParser(const std::string &name); +urdf::ManagedSensorParserMap getSensorParser(const std::string &name); /** parse tags in URDF document */ SensorMap parseSensors(const std::string &xml, const urdf::SensorParserMap &parsers); diff --git a/urdf/src/sensor.cpp b/urdf/src/sensor.cpp index 45b6f461..01188140 100644 --- a/urdf/src/sensor.cpp +++ b/urdf/src/sensor.cpp @@ -37,13 +37,21 @@ #include "urdf/sensor.h" #include -#include #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; @@ -87,13 +95,13 @@ SensorMap parseSensors(const std::string &xml_string, const SensorParserMap &par return parseSensors(xml_doc, parsers); } -SensorParserMap getSensorParsers(const std::vector &allowed) +ManagedSensorParserMap getSensorParsers(const std::vector &allowed) { pluginlib::ClassLoader loader("urdf", "urdf::SensorParser"); - SensorParserMap parserMap; + ManagedSensorParserMap parserMap; try { - const std::vector &classes = loader.getDeclaredClasses(); + 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 @@ -102,7 +110,7 @@ SensorParserMap getSensorParsers(const std::vector &allowed) urdf::SensorParserSharedPtr parser; try { - parser = loader.createInstance(classes[i]); + parser = parserMap.loader->createInstance(classes[i]); } catch(const pluginlib::PluginlibException& ex) { ROS_ERROR_STREAM("Failed to create sensor parser: " << classes[i] << "\n" << ex.what()); } @@ -119,7 +127,7 @@ SensorParserMap getSensorParsers(const std::vector &allowed) return parserMap; } -SensorParserMap getSensorParser(const std::string &name) +ManagedSensorParserMap getSensorParser(const std::string &name) { std::vector allowed; allowed.push_back(name); From 4e13137b0564b1d8061b12803131d362c45be958 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 22 Sep 2022 18:42:04 +0200 Subject: [PATCH 8/8] Migrate boost::std_shared_ptr -> std::std_shared_ptr --- urdf/src/sensor.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/urdf/src/sensor.cpp b/urdf/src/sensor.cpp index 01188140..ca6101b1 100644 --- a/urdf/src/sensor.cpp +++ b/urdf/src/sensor.cpp @@ -37,7 +37,6 @@ #include "urdf/sensor.h" #include -#include #include #include @@ -110,7 +109,7 @@ ManagedSensorParserMap getSensorParsers(const std::vector &allowed) urdf::SensorParserSharedPtr parser; try { - parser = parserMap.loader->createInstance(classes[i]); + parser = parserMap.loader->createUniqueInstance(classes[i]); } catch(const pluginlib::PluginlibException& ex) { ROS_ERROR_STREAM("Failed to create sensor parser: " << classes[i] << "\n" << ex.what()); }