return a filtered list of parsers only

ubi-agni · Oct 7, 2020 · b120b2b · b120b2b
1 parent ac5ec45
commit b120b2b
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 13 deletions.
diff --git a/urdf/include/urdf/sensor.h b/urdf/include/urdf/sensor.h
@@ -44,9 +44,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<std::string> &allowed = std::vector<std::string>());
+
+/** Conveniency method returning the SensorParserMap for the given sensor name */
+urdf::SensorParserMap getSensorParser(const std::string &name);
 
 /** parse <sensor> tags in URDF document */
 SensorMap parseSensors(TiXmlDocument &doc, const urdf::SensorParserMap &parsers);

diff --git a/urdf/src/sensor.cpp b/urdf/src/sensor.cpp
@@ -39,6 +39,7 @@
 #include <ros/ros.h>
 #include <pluginlib/class_loader.h>
 #include <boost/shared_ptr.hpp>
+#include <algorithm>
 #include <fstream>
 
 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<std::string> &allowed)
 {
   static boost::mutex PARSER_PLUGIN_LOCK;
   static boost::shared_ptr<pluginlib::ClassLoader<urdf::SensorParser> > PARSER_PLUGIN_LOADER;
@@ -101,24 +102,35 @@ const SensorParserMap& getDefaultSensorParserMap()
       const std::vector<std::string> &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");
     }
   }
   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<std::string> allowed;
+  allowed.push_back(name);
+  return getSensorParsers(allowed);
+}
+
 } // namespace