-
Notifications
You must be signed in to change notification settings - Fork 6
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Make urdf plugable and revive urdf_parser_plugin #13
Changes from 34 commits
c105a5d
4608277
683c3ff
2058537
535c275
e310409
7250733
7efedb4
288413b
f9b2176
e09b0e1
3adaaba
d079336
41fa3f7
cb348db
4ba3e05
33ad016
7055891
8bb27d1
c9f629d
0076d76
6df48c9
3841ea4
572534d
4f43841
9494566
aac7fd3
67df5dd
deabb1b
70a81cc
3341988
69b3d49
025f816
31014c0
06474e8
f9fe6f9
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -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 | ||
{ | ||
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(); | ||
clalancette marked this conversation as resolved.
Show resolved
Hide resolved
|
||
pluginlib::UniquePtr<urdf::URDFParser> best_plugin; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. There is some possibility that no plugin matches, in which case we wouldn't assign anything to There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Explicit initialize to nullptr in f9fe6f9 |
||
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 | ||
sloretz marked this conversation as resolved.
Show resolved
Hide resolved
|
||
continue; | ||
} | ||
clalancette marked this conversation as resolved.
Show resolved
Hide resolved
|
||
size_t score = plugin_instance->might_handle(data); | ||
sloretz marked this conversation as resolved.
Show resolved
Hide resolved
|
||
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; | ||
} | ||
clalancette marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
||
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nit: this can be a
final
class, since nothing will be deriving from it.There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Final in 06474e8