From: Jochen Sprickerhof <git@jochen.sprickerhof.de>
Date: Sun, 29 May 2022 07:16:08 +0200
Subject: Add ROS 1 API

Mostly copied from pluginlib 1.13.0.
---
 pluginlib/CMakeLists.txt                         |  2 +
 pluginlib/include/pluginlib/class_loader.hpp     | 22 ++++++++
 pluginlib/include/pluginlib/class_loader_imp.hpp | 67 ++++++++++++++++++++++++
 3 files changed, 91 insertions(+)

diff --git a/pluginlib/CMakeLists.txt b/pluginlib/CMakeLists.txt
index c9bd62e..c65b8e7 100644
--- a/pluginlib/CMakeLists.txt
+++ b/pluginlib/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(class_loader REQUIRED)
 find_package(rcutils REQUIRED)
 find_package(rcpputils REQUIRED)
 find_package(TinyXML2 REQUIRED)
+find_package(roslib REQUIRED)
 
 add_library(${PROJECT_NAME} INTERFACE)
 target_include_directories(${PROJECT_NAME} INTERFACE
@@ -27,6 +28,7 @@ target_link_libraries(${PROJECT_NAME} INTERFACE
   class_loader::class_loader
   rcutils::rcutils
   rcpputils::rcpputils
+  roslib
   tinyxml2::tinyxml2)
 
 add_executable(list_plugins src/list_plugins.cpp)
diff --git a/pluginlib/include/pluginlib/class_loader.hpp b/pluginlib/include/pluginlib/class_loader.hpp
index 28eb0f0..96d34df 100644
--- a/pluginlib/include/pluginlib/class_loader.hpp
+++ b/pluginlib/include/pluginlib/class_loader.hpp
@@ -34,12 +34,26 @@
 #include <string>
 #include <vector>
 
+#include <boost/shared_ptr.hpp>
 #include "class_loader/multi_library_class_loader.hpp"
 #include "pluginlib/class_desc.hpp"
 #include "pluginlib/class_loader_base.hpp"
 #include "pluginlib/exceptions.hpp"
 #include "tinyxml2.h"  // NOLINT
 
+namespace
+{
+
+template<class SP>
+struct Holder
+{
+    SP sp;
+    explicit Holder (const SP& p) : sp(p) {}
+    void operator()(...) { sp.reset(); }
+};
+
+}
+
 namespace pluginlib
 {
 
@@ -84,6 +98,8 @@ public:
    */
   std::shared_ptr<T> createSharedInstance(const std::string & lookup_name);
 
+  boost::shared_ptr<T> createInstance(const std::string & lookup_name);
+
   /// Create an instance of a desired class.
   /**
    * Implicitly calls loadLibraryForClass() to increment the library counter.
@@ -261,6 +277,9 @@ private:
     const std::string & library_name,
     const std::string & exporting_package_name);
 
+  /// Return the paths where libraries are installed according to the Catkin build system.
+  std::vector<std::string> getCatkinLibraryPaths();
+
   /// Return an error message for an unknown class.
   /**
    * \param lookup_name The name of the class
@@ -271,6 +290,9 @@ private:
   /// Get the standard path separator for the native OS (e.g. "/" on *nix, "\" on Windows).
   std::string getPathSeparator();
 
+  /// Given a package name, return the path where rosbuild thinks plugins are installed.
+  std::string getROSBuildLibraryPath(const std::string & exporting_package_name);
+
   /// Get the package name from a path to a plugin XML file.
   std::string getPackageFromPluginXMLFilePath(const std::string & path);
 
diff --git a/pluginlib/include/pluginlib/class_loader_imp.hpp b/pluginlib/include/pluginlib/class_loader_imp.hpp
index 5d0f73a..9f617bc 100644
--- a/pluginlib/include/pluginlib/class_loader_imp.hpp
+++ b/pluginlib/include/pluginlib/class_loader_imp.hpp
@@ -46,6 +46,7 @@
 #include "ament_index_cpp/get_resource.hpp"
 #include "ament_index_cpp/get_resources.hpp"
 #include "class_loader/class_loader.hpp"
+#include <ros/package.h>
 #include "rcpputils/shared_library.hpp"
 #include "rcutils/logging_macros.h"
 
@@ -83,7 +84,9 @@ ClassLoader<T>::ClassLoader(
     ament_index_cpp::get_package_prefix(package_);
   } catch (const ament_index_cpp::PackageNotFoundError & exception) {
     // rethrow as class loader exception, package name is in the error message already.
+    if (ros::package::getPath(package_).empty()) {
     throw pluginlib::ClassLoaderException(exception.what());
+    }
   }
 
   if (0 == plugin_xml_paths_.size()) {
@@ -111,6 +114,14 @@ std::shared_ptr<T> ClassLoader<T>::createSharedInstance(const std::string & look
   return createUniqueInstance(lookup_name);
 }
 
+template<class T>
+boost::shared_ptr<T> ClassLoader<T>::createInstance(const std::string & lookup_name)
+/***************************************************************************/
+{
+  std::shared_ptr<T> p = createSharedInstance(lookup_name);
+  return boost::shared_ptr<T>(p.get(), Holder<std::shared_ptr<T>>(p));
+}
+
 template<class T>
 UniquePtr<T> ClassLoader<T>::createUniqueInstance(const std::string & lookup_name)
 {
@@ -218,6 +229,9 @@ std::vector<std::string> ClassLoader<T>::getPluginXmlPaths(
       }
     }
   }
+  if (paths.size() == 0) {
+    ros::package::getPlugins(package, attrib_name, paths);
+  }
   return paths;
 }
 
@@ -288,6 +302,24 @@ std::string ClassLoader<T>::extractPackageNameFromPackageXML(const std::string &
   return package_name_node_txt;
 }
 
+template<class T>
+std::vector<std::string> ClassLoader<T>::getCatkinLibraryPaths()
+/***************************************************************************/
+{
+  std::vector<std::string> lib_paths;
+  const char * env = std::getenv("CMAKE_PREFIX_PATH");
+  if (env) {
+    std::stringstream ss(env);
+    std::string catkin_prefix_path;
+    while (std::getline(ss, catkin_prefix_path, *CLASS_LOADER_IMPL_OS_PATHSEP)) {
+      std::filesystem::path path(catkin_prefix_path);
+      std::filesystem::path lib("lib");
+      lib_paths.push_back((path / lib).string());
+    }
+  }
+  return lib_paths;
+}
+
 template<class T>
 std::vector<std::string> ClassLoader<T>::getAllLibraryPathsToTry(
   const std::string & library_name,
@@ -325,6 +357,7 @@ std::vector<std::string> ClassLoader<T>::getAllLibraryPathsToTry(
 
   std::vector<std::string> all_paths;  // result of all pairs to search
 
+  try {
   std::string package_prefix = ament_index_cpp::get_package_prefix(exporting_package_name);
 
   // Setup the directories to look in.
@@ -382,6 +415,33 @@ std::vector<std::string> ClassLoader<T>::getAllLibraryPathsToTry(
       library_name.c_str(),
       path.c_str());
   }
+  } catch (const ament_index_cpp::PackageNotFoundError & exception) {
+  // Catkin-rosbuild Backwards Compatability Rules - Note library_name may be prefixed with
+  // relative path (e.g. "/lib/libFoo")
+  // 1. Try catkin library paths (catkin_find --libs) + library_name + extension
+  // 2. Try catkin library paths
+  //   (catkin_find -- libs) + stripAllButFileFromPath(library_name) + extension
+  // 3. Try export_pkg/library_name + extension
+
+  std::vector<std::string> all_paths_without_extension = getCatkinLibraryPaths();
+  all_paths_without_extension.push_back(getROSBuildLibraryPath(exporting_package_name));
+  std::string non_debug_suffix = ".so";
+  std::string library_name_with_extension = library_name + non_debug_suffix;
+  std::string stripped_library_name = stripAllButFileFromPath(library_name);
+  std::string stripped_library_name_with_extension = stripped_library_name + non_debug_suffix;
+
+  const std::string path_separator = getPathSeparator();
+
+  for (unsigned int c = 0; c < all_paths_without_extension.size(); c++) {
+    std::string current_path = all_paths_without_extension.at(c);
+    all_paths.push_back(current_path + path_separator + library_name_with_extension);
+    all_paths.push_back(current_path + path_separator + stripped_library_name_with_extension);
+  }
+  }
+
+  if (all_paths.size() == 0) {
+    ament_index_cpp::get_package_prefix(exporting_package_name);
+  }
 
   return all_paths;
 }
@@ -591,6 +651,13 @@ std::vector<std::string> ClassLoader<T>::getRegisteredLibraries()
   return lowlevel_class_loader_.getRegisteredLibraries();
 }
 
+template<class T>
+std::string ClassLoader<T>::getROSBuildLibraryPath(const std::string & exporting_package_name)
+/***************************************************************************/
+{
+  return ros::package::getPath(exporting_package_name);
+}
+
 template<class T>
 bool ClassLoader<T>::isClassAvailable(const std::string & lookup_name)
 /***************************************************************************/