From dab0a4e9d627e70b9a7dc053bc087693bb1eef82 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Wed, 21 Sep 2022 10:14:40 +0200
Subject: [PATCH] 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 <string>
 #include <map>
 #include <urdf_parser/sensor_parser.h>
+#include <pluginlib/class_loader.h>
 
 namespace urdf {
 
+// Maintain class loader together with created parser instances
+class ManagedSensorParserMap : public SensorParserMap {
+public:
+    std::unique_ptr<pluginlib::ClassLoader<urdf::SensorParser>> 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<std::string> &allowed = std::vector<std::string>());
+urdf::ManagedSensorParserMap getSensorParsers(const std::vector<std::string> &allowed = std::vector<std::string>());
 
 /** 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 <sensor> 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 <ros/ros.h>
-#include <pluginlib/class_loader.h>
 #include <boost/shared_ptr.hpp>
 #include <algorithm>
 #include <fstream>
 
 namespace urdf {
 
+ManagedSensorParserMap::ManagedSensorParserMap()
+  : loader(new pluginlib::ClassLoader<urdf::SensorParser>("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<std::string> &allowed)
+ManagedSensorParserMap getSensorParsers(const std::vector<std::string> &allowed)
 {
   pluginlib::ClassLoader<urdf::SensorParser> loader("urdf", "urdf::SensorParser");
-  SensorParserMap parserMap;
+  ManagedSensorParserMap parserMap;
   try
   {
-    const std::vector<std::string> &classes = loader.getDeclaredClasses();
+    const std::vector<std::string> &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<std::string> &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<std::string> &allowed)
   return parserMap;
 }
 
-SensorParserMap getSensorParser(const std::string &name)
+ManagedSensorParserMap getSensorParser(const std::string &name)
 {
   std::vector<std::string> allowed;
   allowed.push_back(name);