Make urdf plugable and revive urdf_parser_plugin

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}
   "$<INSTALL_INTERFACE:include>"
 )
 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()

urdf/include/urdf/model.h

@@ -37,6 +37,7 @@
 #ifndef URDF__MODEL_H_
 #define URDF__MODEL_H_
 
+#include <memory>
 #include <string>
 
 #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<ModelImplementation> impl_;
 };
 
 // shared_ptr declarations moved to urdf/urdfdom_compatibility.h to allow for

urdf/package.xml

@@ -21,22 +21,29 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+  <build_depend>pluginlib</build_depend>
   <build_depend>tinyxml</build_depend>
   <build_depend>tinyxml_vendor</build_depend>
   <build_depend>urdfdom</build_depend>
+  <build_depend>urdf_parser_plugin</build_depend>
   <!-- use ROS 2 package urdfdom_headers until upstream provides 1.0.0.-->
   <build_depend>urdfdom_headers</build_depend>
 
+  <exec_depend>pluginlib</exec_depend>
   <exec_depend>tinyxml</exec_depend>
   <exec_depend>tinyxml_vendor</exec_depend>
   <exec_depend>urdfdom</exec_depend>
   <!-- use ROS 2 package urdfdom_headers until upstream provides 1.0.0.-->
   <exec_depend>urdfdom_headers</exec_depend>
 
+  <build_export_depend>pluginlib</build_export_depend>
   <build_export_depend>tinyxml</build_export_depend>
+  <build_export_depend>urdf_parser_plugin</build_export_depend>
+  <build_export_depend>urdfdom</build_export_depend>
   <!-- use ROS 2 package urdfdom_headers until upstream provides 1.0.0.-->
   <build_export_depend>urdfdom_headers</build_export_depend>
 
+  <test_depend>ament_cmake_google_benchmark</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

urdf/src/model.cpp

@@ -34,23 +34,47 @@
 
 /* Author: Wim Meeussen */
 
+#include "urdf/model.h"
+#include <urdf_parser_plugin/parser.h>
+#include <pluginlib/class_loader.hpp>
+
+#include <cassert>
 #include <fstream>
 #include <iostream>
+#include <limits>
 #include <string>
+#include <utility>
 #include <vector>
 
-#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<urdf::URDFParser> load_plugin(const std::string & plugin_name);
+
+  // Loader used to get plugins
+  pluginlib::ClassLoader<urdf::URDFParser> loader_;
+};
+
+
+Model::Model()
+: impl_(new ModelImplementation)
+{
+}
 
-static bool IsColladaData(const std::string & data)
+Model::~Model()
 {
-  return data.find("<COLLADA") != std::string::npos;
+  clear();
+  impl_.reset();
 }
 
 bool Model::initFile(const std::string & filename)
@@ -124,59 +148,67 @@ bool Model::initXml(TiXmlElement * robot_xml)
   return Model::initString(ss.str());
 }
 
-bool Model::initString(const std::string & xml_string)
+pluginlib::UniquePtr<urdf::URDFParser>
+ModelImplementation::load_plugin(const std::string & plugin_name)
+{
+  pluginlib::UniquePtr<urdf::URDFParser> 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<pluginlib::ClassLoader<urdf::URDFParser> > PARSER_PLUGIN_LOADER;
-    boost::mutex::scoped_lock _(PARSER_PLUGIN_LOCK);
-
-    try
-    {
-      if (!PARSER_PLUGIN_LOADER)
-        PARSER_PLUGIN_LOADER.reset(new pluginlib::ClassLoader<urdf::URDFParser>("urdf_parser_plugin", "urdf::URDFParser"));
-     const std::vector<std::string> &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<urdf::URDFParser> 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<size_t>::max();
+  auto best_plugin = pluginlib::UniquePtr<urdf::URDFParser>{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<urdf::URDFParser> 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