diff --git a/urdf/include/urdf/sensor.h b/urdf/include/urdf/sensor.h index 150600e7..a87d1977 100644 --- 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 &allowed = std::vector()); + +/** Conveniency method returning the SensorParserMap for the given sensor name */ +urdf::SensorParserMap getSensorParser(const std::string &name); /** parse tags in URDF document */ SensorMap parseSensors(TiXmlDocument &doc, const urdf::SensorParserMap &parsers); diff --git a/urdf/src/sensor.cpp b/urdf/src/sensor.cpp index 3b7897d7..08a9a1e3 100644 --- a/urdf/src/sensor.cpp +++ b/urdf/src/sensor.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include 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 &allowed) { static boost::mutex PARSER_PLUGIN_LOCK; static boost::shared_ptr > PARSER_PLUGIN_LOADER; @@ -101,14 +102,18 @@ const SensorParserMap& getDefaultSensorParserMap() const std::vector &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"); @@ -116,9 +121,16 @@ const SensorParserMap& getDefaultSensorParserMap() } 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 allowed; + allowed.push_back(name); + return getSensorParsers(allowed); +} + } // namespace