From 4b73ae2998bec0db24aca07b0bf7fc37b8e4dae7 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 18 Sep 2020 13:06:11 -0700 Subject: [PATCH] Make urdf plugable and revive urdf_parser_plugin (#13) * Make urdf plugable This makes the urdf package load parser plugins. It includes a new api might_handle() that returns a score for how likely the plugin is to be the one the given file format is meant for. This change also makes the urdf xml parser a plugin instead of a special case that is called directly. This breaks ABI with urdf::Model because the model now stores the class loader instance. Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz * Restore dependency on urdfdom Signed-off-by: Shane Loretz * Style Signed-off-by: Shane Loretz * Stops crash; not sure why TODO Signed-off-by: Shane Loretz * Add benchmark showing plugin overhead Signed-off-by: Shane Loretz * Whitespace Signed-off-by: Shane Loretz * CMake 3.5 Signed-off-by: Shane Loretz * Include Signed-off-by: Shane Loretz * Remove buildtool_export on ament_cmake Signed-off-by: Shane Loretz * exec_depend -> build_export_depend urdfdom headers Signed-off-by: Shane Loretz * Remove commented code Signed-off-by: Shane Loretz * Document urdf_parser_plugin usage Signed-off-by: Shane Loretz * Document PIMPL forward declaration Signed-off-by: Shane Loretz * Alphabetize dependencies Signed-off-by: Shane Loretz * Use tinyxml2 to reduce false positive might_handle() Signed-off-by: Shane Loretz * Update urdf/src/urdf_plugin.cpp Signed-off-by: Shane Loretz Co-authored-by: Chris Lalancette * Handle pluginlib exceptions Signed-off-by: Shane Loretz * Return early on failure Signed-off-by: Shane Loretz * Document size_t max is no confidence score Signed-off-by: Shane Loretz * Remove debut print Signed-off-by: Shane Loretz * Move urdfdom_headers comment one line below Signed-off-by: Shane Loretz * Avoid using nullptr in release mode Signed-off-by: Shane Loretz * nonvirtual dtor final class Signed-off-by: Shane Loretz * Use ROS 2 urdfdom_headers Signed-off-by: Shane Loretz * Remove unused exec variable Signed-off-by: Shane Loretz * Skip if xml fails to parse Signed-off-by: Shane Loretz * Make sure test can find pluginlib plugin Signed-off-by: Shane Loretz * Use SHARED instead of module Fixes OSX failing build https://github.com/ros/pluginlib/issues/200 Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz * picked -> chosen Signed-off-by: Shane Loretz * Use pluginlib_enable_plugin_testing() Signed-off-by: Shane Loretz * Define might_handle() return to length of data Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz * Return data.size() when not confident Signed-off-by: Shane Loretz * Try urdf when no plugin is confident Signed-off-by: Shane Loretz * ModelImplementation final Signed-off-by: Shane Loretz * Initialize best_plugin to nullptr Signed-off-by: Shane Loretz Co-authored-by: Chris Lalancette --- urdf/CMakeLists.txt | 43 ++++++ urdf/include/urdf/model.h | 26 +++- urdf/package.xml | 7 + urdf/src/model.cpp | 138 +++++++++++------- urdf/src/urdf_plugin.cpp | 81 ++++++++++ urdf/test/benchmark_plugin_overhead.cpp | 87 +++++++++++ urdf/urdf_parser_description.xml | 7 + urdf_parser_plugin/AMENT_IGNORE | 0 urdf_parser_plugin/CMakeLists.txt | 24 ++- .../include/urdf_parser_plugin/parser.h | 20 ++- urdf_parser_plugin/package.xml | 13 +- 11 files changed, 374 insertions(+), 72 deletions(-) create mode 100644 urdf/src/urdf_plugin.cpp create mode 100644 urdf/test/benchmark_plugin_overhead.cpp create mode 100644 urdf/urdf_parser_description.xml delete mode 100644 urdf_parser_plugin/AMENT_IGNORE diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index 3d8a3a3c..e5e1ebb8 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.5) project(urdf) find_package(ament_cmake_ros REQUIRED) +find_package(pluginlib REQUIRED) +find_package(urdf_parser_plugin REQUIRED) find_package(urdfdom REQUIRED) find_package(urdfdom_headers REQUIRED) find_package(tinyxml_vendor REQUIRED) @@ -33,8 +35,10 @@ target_include_directories(${PROJECT_NAME} "$" ) ament_target_dependencies(${PROJECT_NAME} + urdf_parser_plugin urdfdom urdfdom_headers + pluginlib TinyXML) if(WIN32) @@ -45,6 +49,22 @@ if(APPLE) set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup") endif() +add_library(urdf_xml_parser SHARED + src/urdf_plugin.cpp +) +target_link_libraries(urdf_xml_parser + ${PROJECT_NAME} +) +ament_target_dependencies(urdf_xml_parser + "pluginlib" + "urdf_parser_plugin" +) + +install(TARGETS urdf_xml_parser + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -56,6 +76,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ if(BUILD_TESTING) find_package(ament_cmake_cppcheck REQUIRED) find_package(ament_cmake_cpplint REQUIRED) + find_package(ament_cmake_google_benchmark REQUIRED) find_package(ament_cmake_lint_cmake REQUIRED) find_package(ament_cmake_uncrustify REQUIRED) # This forces cppcheck to consider all files in this project to be C++, @@ -65,13 +86,35 @@ if(BUILD_TESTING) ament_cpplint() ament_lint_cmake() ament_uncrustify(LANGUAGE "C++") + + pluginlib_enable_plugin_testing( + CMAKE_TARGET_VAR mock_install_target + AMENT_PREFIX_PATH_VAR mock_install_path + PLUGIN_CATEGORY "urdf_parser" + PLUGIN_DESCRIPTIONS "urdf_parser_description.xml" + PLUGIN_LIBRARIES urdf_xml_parser + ) + + ament_add_google_benchmark(plugin_overhead + test/benchmark_plugin_overhead.cpp + APPEND_ENV AMENT_PREFIX_PATH="${mock_install_path}" + ) + target_link_libraries(plugin_overhead + urdf + ) + add_dependencies(plugin_overhead "${mock_install_target}") endif() ament_export_libraries(${PROJECT_NAME}) ament_export_targets(${PROJECT_NAME}) ament_export_include_directories(include) +ament_export_dependencies(pluginlib) ament_export_dependencies(tinyxml_vendor) ament_export_dependencies(TinyXML) +ament_export_dependencies(urdf_parser_plugin) ament_export_dependencies(urdfdom) ament_export_dependencies(urdfdom_headers) + +pluginlib_export_plugin_description_file(urdf_parser_plugin "urdf_parser_description.xml") + ament_package() diff --git a/urdf/include/urdf/model.h b/urdf/include/urdf/model.h index 1a01d487..cb56bae8 100644 --- a/urdf/include/urdf/model.h +++ b/urdf/include/urdf/model.h @@ -37,6 +37,7 @@ #ifndef URDF__MODEL_H_ #define URDF__MODEL_H_ +#include #include #include "tinyxml.h" // NOLINT @@ -48,24 +49,39 @@ namespace urdf { +// PIMPL Forward Declaration +class ModelImplementation; + +/// \brief Populates itself based on a robot descripton +/// +/// This class uses `urdf_parser_plugin` to parse the given robot description. +/// The chosen plugin is the one that reports the most confident score. +/// There is no way to override this choice except by uninstalling undesirable +/// parser plugins. class Model : public ModelInterface { public: + URDF_EXPORT + Model(); + + URDF_EXPORT + ~Model(); + /// \brief Load Model from TiXMLElement [[deprecated("use initString instead")]] URDF_EXPORT bool initXml(TiXmlElement * xml); /// \brief Load Model from TiXMLDocument [[deprecated("use initString instead")]] URDF_EXPORT bool initXml(TiXmlDocument * xml); + /// \brief Load Model given a filename URDF_EXPORT bool initFile(const std::string & filename); - /// \brief Load Model given the name of a parameter on the parameter server - // URDF_EXPORT bool initParam(const std::string & param); - /// \brief Load Model given the name of parameter on parameter server using provided nodehandle - // URDF_EXPORT bool initParamWithNodeHandle(const std::string & param, - // const ros::NodeHandle & nh = ros::NodeHandle()); + /// \brief Load Model from a XML-string URDF_EXPORT bool initString(const std::string & xmlstring); + +private: + std::unique_ptr impl_; }; // shared_ptr declarations moved to urdf/urdfdom_compatibility.h to allow for diff --git a/urdf/package.xml b/urdf/package.xml index 5734dc74..428b30e3 100644 --- a/urdf/package.xml +++ b/urdf/package.xml @@ -21,22 +21,29 @@ ament_cmake_ros + pluginlib tinyxml tinyxml_vendor urdfdom + urdf_parser_plugin urdfdom_headers + pluginlib tinyxml tinyxml_vendor urdfdom urdfdom_headers + pluginlib tinyxml + urdf_parser_plugin + urdfdom urdfdom_headers + ament_cmake_google_benchmark ament_lint_auto ament_lint_common diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp index 29311424..4b1a0412 100644 --- a/urdf/src/model.cpp +++ b/urdf/src/model.cpp @@ -34,23 +34,47 @@ /* Author: Wim Meeussen */ +#include "urdf/model.h" +#include +#include + +#include #include #include +#include #include +#include #include -#include "urdf/model.h" - -/* we include the default parser for plain URDF files; - other parsers are loaded via plugins (if available) */ -#include "urdf_parser/urdf_parser.h" namespace urdf { +class ModelImplementation final +{ +public: + ModelImplementation() + : loader_("urdf_parser_plugin", "urdf::URDFParser") + { + } + + ~ModelImplementation() = default; + + pluginlib::UniquePtr load_plugin(const std::string & plugin_name); + + // Loader used to get plugins + pluginlib::ClassLoader loader_; +}; + + +Model::Model() +: impl_(new ModelImplementation) +{ +} -static bool IsColladaData(const std::string & data) +Model::~Model() { - return data.find(" +ModelImplementation::load_plugin(const std::string & plugin_name) +{ + pluginlib::UniquePtr plugin_instance; + try { + plugin_instance = loader_.createUniqueInstance(plugin_name); + } catch (const pluginlib::CreateClassException &) { + fprintf(stderr, "Failed to load urdf_parser_plugin [%s]\n", plugin_name.c_str()); + } + return std::move(plugin_instance); +} + +bool Model::initString(const std::string & data) { urdf::ModelInterfaceSharedPtr model; - // necessary for COLLADA compatibility - if (IsColladaData(xml_string)) { - fprintf(stderr, "Parsing robot collada xml string is not yet supported.\n"); - return false; - /* - ROS_DEBUG("Parsing robot collada xml string"); - - static boost::mutex PARSER_PLUGIN_LOCK; - static boost::scoped_ptr > PARSER_PLUGIN_LOADER; - boost::mutex::scoped_lock _(PARSER_PLUGIN_LOCK); - - try - { - if (!PARSER_PLUGIN_LOADER) - PARSER_PLUGIN_LOADER.reset(new pluginlib::ClassLoader("urdf_parser_plugin", "urdf::URDFParser")); - const std::vector &classes = PARSER_PLUGIN_LOADER->getDeclaredClasses(); - bool found = false; - for (std::size_t i = 0 ; i < classes.size() ; ++i) - if (classes[i].find("urdf/ColladaURDFParser") != std::string::npos) - { - boost::shared_ptr instance = PARSER_PLUGIN_LOADER->createInstance(classes[i]); - if (instance) - model = instance->parse(xml_string); - found = true; - break; - } - if (!found) - ROS_ERROR_STREAM("No URDF parser plugin found for Collada files. Did you install the corresponding package?"); - } - catch(pluginlib::PluginlibException& ex) - { - ROS_ERROR_STREAM("Exception while creating planning plugin loader " << ex.what() << ". Will not parse Collada file."); - } + size_t best_score = std::numeric_limits::max(); + auto best_plugin = pluginlib::UniquePtr{nullptr}; + std::string best_plugin_name; + + // Figure out what plugins might handle this format + for (const std::string & plugin_name : impl_->loader_.getDeclaredClasses()) { + pluginlib::UniquePtr plugin_instance = impl_->load_plugin(plugin_name); + if (!plugin_instance) { + // Debug mode + assert(plugin_instance); + // Release mode + continue; + } + size_t score = plugin_instance->might_handle(data); + if (score < best_score) { + best_score = score; + best_plugin = std::move(plugin_instance); + best_plugin_name = plugin_name; + } } - */ - } else { - fprintf(stderr, "Parsing robot urdf xml string.\n"); - model = parseURDF(xml_string); + + if (best_score >= data.size()) { + // No plugin was confident ... try urdf anyways + best_plugin_name = "urdf_xml_parser/URDFXMLParser"; + best_plugin = impl_->load_plugin(best_plugin_name); + } + + if (!best_plugin) { + fprintf(stderr, "No plugin found for given robot description.\n"); + return false; } + model = best_plugin->parse(data); + // copy data from model into this object - if (model) { - this->links_ = model->links_; - this->joints_ = model->joints_; - this->materials_ = model->materials_; - this->name_ = model->name_; - this->root_link_ = model->root_link_; - return true; + if (!model) { + fprintf(stderr, "Failed to parse robot description using: %s\n", best_plugin_name.c_str()); + return false; } - return false; + + this->links_ = model->links_; + this->joints_ = model->joints_; + this->materials_ = model->materials_; + this->name_ = model->name_; + this->root_link_ = model->root_link_; + return true; } } // namespace urdf diff --git a/urdf/src/urdf_plugin.cpp b/urdf/src/urdf_plugin.cpp new file mode 100644 index 00000000..3c2f389b --- /dev/null +++ b/urdf/src/urdf_plugin.cpp @@ -0,0 +1,81 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2020, Open Source Robotics Foundation, 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. +*********************************************************************/ + +#include +#include +#include + +#include + +namespace urdf +{ +class URDFXMLParser final : public urdf::URDFParser +{ +public: + URDFXMLParser() = default; + + ~URDFXMLParser() = default; + + urdf::ModelInterfaceSharedPtr parse(const std::string & xml_string) override; + + size_t might_handle(const std::string & data) override; +}; + +urdf::ModelInterfaceSharedPtr URDFXMLParser::parse(const std::string & xml_string) +{ + return urdf::parseURDF(xml_string); +} + +size_t URDFXMLParser::might_handle(const std::string & data) +{ + tinyxml2::XMLDocument doc; + const tinyxml2::XMLError error = doc.Parse(data.c_str()); + if (error == tinyxml2::XML_SUCCESS) { + // Since it's an XML document it must have `` as the first tag + const tinyxml2::XMLElement * root = doc.RootElement(); + if (std::string("robot") != root->Name()) { + return data.size(); + } + } + + // Possiblities: + // 1) It is not an XML based robot description + // 2) It is an XML based robot description, but there's an XML syntax error + // 3) It is a URDF XML with correct XML syntax + return data.find(" // NOLINT +PLUGINLIB_EXPORT_CLASS(urdf::URDFXMLParser, urdf::URDFParser) diff --git a/urdf/test/benchmark_plugin_overhead.cpp b/urdf/test/benchmark_plugin_overhead.cpp new file mode 100644 index 00000000..d7c34f85 --- /dev/null +++ b/urdf/test/benchmark_plugin_overhead.cpp @@ -0,0 +1,87 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2020, 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. +*********************************************************************/ + +#include +#include + +#include + +#include "urdf/model.h" + +const char test_xml[] = + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + ""; + +static void BM_no_plugin(benchmark::State & state) +{ + for (auto _ : state) { + if (nullptr == urdf::parseURDF(test_xml)) { + state.SkipWithError("Failed to read xml"); + break; + } + } +} + +static void BM_with_plugin(benchmark::State & state) +{ + for (auto _ : state) { + urdf::Model m; + if (!m.initString(test_xml)) { + state.SkipWithError("Failed to read xml"); + break; + } + } +} + +BENCHMARK(BM_no_plugin); +BENCHMARK(BM_with_plugin); + +BENCHMARK_MAIN(); diff --git a/urdf/urdf_parser_description.xml b/urdf/urdf_parser_description.xml new file mode 100644 index 00000000..c4b8b377 --- /dev/null +++ b/urdf/urdf_parser_description.xml @@ -0,0 +1,7 @@ + + + + Parse models as URDF from URDF XML. + + + diff --git a/urdf_parser_plugin/AMENT_IGNORE b/urdf_parser_plugin/AMENT_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/urdf_parser_plugin/CMakeLists.txt b/urdf_parser_plugin/CMakeLists.txt index 75c68547..be8c1f1f 100644 --- a/urdf_parser_plugin/CMakeLists.txt +++ b/urdf_parser_plugin/CMakeLists.txt @@ -1,13 +1,23 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(urdf_parser_plugin) -find_package(catkin REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(urdfdom_headers REQUIRED) -catkin_package( - INCLUDE_DIRS include - DEPENDS urdfdom_headers +# Create and export a header-only target +add_library(urdf_parser_plugin INTERFACE) +target_include_directories(urdf_parser_plugin INTERFACE + $ +) +ament_target_dependencies(urdf_parser_plugin INTERFACE + "urdfdom_headers" +) +install(TARGETS urdf_parser_plugin EXPORT urdf_parser_plugin-export) +ament_export_targets(urdf_parser_plugin-export) + +install( + DIRECTORY include/ + DESTINATION include ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +ament_package() diff --git a/urdf_parser_plugin/include/urdf_parser_plugin/parser.h b/urdf_parser_plugin/include/urdf_parser_plugin/parser.h index 92de4113..abde3c42 100644 --- a/urdf_parser_plugin/include/urdf_parser_plugin/parser.h +++ b/urdf_parser_plugin/include/urdf_parser_plugin/parser.h @@ -37,13 +37,15 @@ #ifndef URDF_PARSER_PLUGIN_H #define URDF_PARSER_PLUGIN_H -#include +#include + +#include namespace urdf { /** \brief Base class for URDF parsers */ -class URDFParser +class URDFParser { public: URDFParser() @@ -54,7 +56,19 @@ class URDFParser } /// \brief Load Model from string - virtual urdf::ModelInterfaceSharedPtr parse(const std::string &xml_string) = 0; + /// \return nullptr and write to stderr if the given string is invalid + virtual urdf::ModelInterfaceSharedPtr parse(const std::string & data) = 0; + + /// \brief Indicate if data is meant to be parsed by this parser + /// \return The position in the string that the plugin became confident the + /// data is intended to be parsed by it. + /// For example, the plugin parsing COLLADA files might return the + /// position in the string that the '' xml tag was found. + /// Smaller values are interpretted as more confidence, and the + /// plugin with the smallest value is used to parse the data. + /// If a plugin believes data is not meant for it, then it should + /// return a value greater than or equal to data.size(). + virtual size_t might_handle(const std::string & data) = 0; }; } diff --git a/urdf_parser_plugin/package.xml b/urdf_parser_plugin/package.xml index 5830ae0a..3d14249a 100644 --- a/urdf_parser_plugin/package.xml +++ b/urdf_parser_plugin/package.xml @@ -1,4 +1,4 @@ - + urdf_parser_plugin 2.2.0 @@ -16,8 +16,13 @@ https://github.com/ros2/urdf https://github.com/ros2/urdf/issues - catkin - liburdfdom-headers-dev - liburdfdom-headers-dev + ament_cmake_ros + + urdfdom_headers + urdfdom_headers + + + ament_cmake +