Proposal: SDFormat URDF Parser Plugin

[IanTheEngineer]

As a user of the ROS ecosystem (robot_state_publisher, rviz, etc.), I can currently choose between two file formats to load into a urdf::Model data structure: URDF or Collada.

I propose we add a third valid file format: SDFormat. SDFormat is used by large robotics software projects like Gazebo and Drake. The source code for SDFormat: https://github.com/osrf/sdformat

To be completely clear, this request is not to require urdf::Model to support all of the possible permutations of SDFormat tags and topologies, but whatever subset makes sense to fufill the Model API's.

The plugin structure is already present and available via urdf_parser_plugin. I suggest we add an additional package (very similar to collada_parser) : sdformat_parser

At first glance, it seems this would require a sdformat_parser package to perform the SDFormat file -> urdf::Model validation and API population. This new package would supply a Class Plugin: urdf/SDFormatURDFParser.

Then we would just need to add a small amount of extra logic inside model.cpp's urdf::Model::initString() function to detect the presence of an SDFormat file and invoke the correct plugin class:

urdf/urdf/src/model.cpp

Lines 164 to 214 in 4707296

	bool Model::initString(const std::string & xml_string)
	{
	urdf::ModelInterfaceSharedPtr model;
	// necessary for COLLADA compatibility
	if (IsColladaData(xml_string)) {
	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.");
	}
	} else {
	ROS_DEBUG("Parsing robot urdf xml string");
	model = parseURDF(xml_string);
	}
	// 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;
	}
	return false;
	}

The intent is not to replace URDF or Collada file formats, but to add a third viable file format option for use with urdf::Model. With all that said, it seems we can support SDFormat without being disruptive to the rest of the ROS ecosystem.

[IanTheEngineer]

FYI
@Levi-Armstrong & @davetcoleman for community visibility
@azeey, @scpeters & @EricCousineau-TRI for SDFormat
@clalancette & @sloretz maintainers

[EricCousineau-TRI]

Sounds great!

Just to check, is there a minimum set of people that you'd like to hear back on (as a response / upvote in this issue) to consider the proposal accepted?

And regarding breakdown, I totally agree with your point in gazebosim/sdformat#265 (an issue which would fall under this proposal)! Do you have an idea of how you'd like to track this as an "epic"? Could we add the relevant subtasks as checkboxes to this issue? (or is too early to consider that?)

[clalancette]

Overall, I like the idea a lot. It adds a new type to the ecosystem while providing backwards compatibility. I think in the future we will probably have to expand "URDF the API" to support more of the features of SDFormat, but we can cross that bridge as we need to. One technical point below.

Then we would just need to add a small amount of extra logic inside model.cpp's urdf::Model::initString() function to detect the presence of an SDFormat file and invoke the correct plugin class:

I have to say that I don't love this practice. It means that there has to be custom logic inside model.cpp for every format we have. But here's the thing; we already have plugins here. We can add a new method to the plugin API like isThisXMLForMe?(). We can then iterate through the registered plugins, calling that and stopping at the first one that returns true. If none of them returns true, we can fall back to URDF mode (which doesn't use a plugin at the moment, I don't think). This is a bit of extra work, and somewhat orthogonal to what you are doing, but it is related. What do you think?

[scpeters]

we already have plugins here. We can add a new method to the plugin API like isThisXMLForMe?(). We can then iterate through the registered plugins, calling that and stopping at the first one that returns true. If none of them returns true, we can fall back to URDF mode

that sounds reasonable to me

[gavanderhoorn]

Slightly more complex: let each plugin return a confidence value and use the one with the highest value to parse the file.

Not sure whether that should include a fall-back strategy (ie: highest confidence fails, try next in line).

[IanTheEngineer]

@clalancette, it's not much more work to add a "isThisXMLForMe?()" method to each of the collada_parser and sdformat_parser, but requires a bit more merge/release coordination. I'll hopefully need only one additional pull request.

This proposed distributed decision making does mean we're ceding control for URDF parsing to each of the ecosystem plugins. If any one of them is greedy and snatches up URDF files (intentionally or not), the URDF parser will be starved under the current logic.

Slightly more complex: let each plugin return a confidence value and use the one with the highest value to parse the file.
Not sure whether that should include a fall-back strategy (ie: highest confidence fails, try next in line).

This is a really interesting idea, @gavanderhoorn. However, I think I'll leave it for future work. It's worth an issue ticket.

[clalancette]

This proposed distributed decision making does mean we're ceding control for URDF parsing to each of the ecosystem plugins. If any one of them is greedy and snatches up URDF files (intentionally or not), the URDF parser will be starved under the current logic.

Yeah, its definitely a risk. But given that we've only had 2 (and now 3) parsers in 10+ years, I think the likelihood of these proliferating and causing that problem is low.

[Levi-Armstrong]

Does this mean that a developer must choose between Collada, URDF or SRDF? Would it support including multiple different formats within the same file; for example when using the include tag?

[clalancette]

Does this mean that a developer must choose between Collada, URDF or SRDF?
Would it support including multiple different formats within the same file; for example when using the include tag?

In some sense, there is nothing preventing the parsers from doing that, before or after this change. The "detection" logic really just looks at some header/initial XML tag information to see if it is of the right type. If it is, then it claims it. Whether that parser then supports "including" other file formats is completely up to the parser.

That being said, I don't believe that any of the COLLADA, URDF, or SDF parsers currently support this.

[EricCousineau-TRI]

Package name suggestions:

• sdformat_to_urdfdom_parser
• sdformat_urdfdom_parser_plugin
• sdformat_to_urdf_parser (yuck?)

Just want to avoid the generic name, like collada_parser / sdformat_parser.

[scpeters]

I'm going to lose all naming privileges for this, but since it's like parsing in reverse, we could call it a resrap

[Levi-Armstrong]

Although I think it is great to add additional support for other formats, but I am not sure what adding SDF parser gets us if the underlining format does not support the much needed features. If the plan is to stick with URDF which I am not against, it may be more beneficial to establish an official specification for the URDF format before adding support for other formats.

[clalancette]

Although I think it is great to add additional support for other formats, but I am not sure what adding SDF parser gets us if the underlining format does not support the much needed features. If the plan is to stick with URDF which I am not against, it may be more beneficial to establish an official specification for the URDF format before adding support for other formats.

@Levi-Armstrong I think the problem here is one of naming. The name "URDF" is overloaded in the ROS ecosystem. It pertains to two different things:

1. The XML format for specifying a robot model (essentially https://github.com/ros/urdfdom/blob/master/xsd/urdf.xsd).
2. The API that presents the entire robot_model to the rest of the ecosystem (basically the API as specified in https://github.com/ros/urdfdom_headers/

Currently, the ecosystem supports parsing both URDF and COLLADA for the first item, presenting the results of either of them via the URDF API. What is being proposed here is to also add a SDF parser to that group, and then exposing the data via the URDF API.

One thing to be totally clear about is that the URDF API, as it stands, cannot represent all of SDF (or COLLADA, for that matter). But I think we can expand that URDF API over time to accommodate more of SDF as needed.

[Levi-Armstrong]

One thing to be totally clear about is that the URDF API, as it stands, cannot represent all of SDF (or COLLADA, for that matter). But I think we can expand that URDF API over time to accommodate more of SDF as needed.

This is was the main point I was trying to make. I think it would be best to expand the API and XML format first then incorporate new parsers. I think we have the opportunity to make breaking changes and if we do not take this chance then it most likely will never evolve given past history.

I believe the ROS approach is to only make breaking changes between distros, but not sure if that is still the case for ROS2. If this is still the case then that most likely means that it will be at least year before it makes it into the release version. If we wait to expand the API and XML format it will most likely be a few years before it every makes into a release version.

[clalancette]

This is was the main point I was trying to make. I think it would be best to expand the API and XML format first then incorporate new parsers. I think we have the opportunity to make breaking changes and if we do not take this chance then it most likely will never evolve given past history.

I guess I don't understand, then.

What is being proposed here is a completely new (to ROS) XML format for specifying statics and dynamics. I think that would fall under your "expanding the XML format" category above. Once that is in, the next step may be to deprecate URDF-the-XML format, and officially freeze that. (Whether and how we take that step is up in the air)

Orthogonal to this is expanding URDF-the-API so that it can expose/express more of the relationships in any of the formats that we support.

Both are valuable, in my opinion, but neither blocks the other.

[sloretz]

This proposed distributed decision making does mean we're ceding control for URDF parsing to each of the ecosystem plugins. If any one of them is greedy and snatches up URDF files (intentionally or not), the URDF parser will be starved under the current logic.

I think this can be mitigated by only installing the plugins needed, though I wouldn't rule out also using heuristic here.

We can add a new method to the plugin API like isThisXMLForMe?(). We can then iterate through the registered plugins, calling that and stopping at the first one that returns true. If none of them returns true, we can fall back to URDF mode

This sounds like a good idea for ROS 2 G-turtle and beyond, though there is some motivation to get this into Melodic and Noetic.
IIUC a new virtual function isThisXMLForMe() in urdf_parser_plugin would break ABI.

I'm guessing the current heuristic is to avoid console spam.
There is only one API urdf::ModelInterfaceSharedPtr parse(const std::string &) that either returns a pointer or returns NULL after printing errors.
The only way I see to test a file is to call parse() and check the return value, but that would print a lot of console spam for each parser that didn't handle the file.
The current code avoids the issue by choosing the parser based on the text <COLLADA being found, otherwise it calls the urdf parser.
isThisXMLForMe() would instead solve the console spam problem by allowing plugins to be tested without calling parse().

For existing distros the only solution I see is to keep using a heuristic to guess the parsr.
I don't think the current heuristic would work if a urdf contained the string <![CDATA[<COLLADA]]>.
That and this could be fixed if the heuristic was just a hint about the order of which parsers to test.

Say urdf used the strategy of trying each parser until one worked, but it used a heuristic to pick the order that it would try. If it sorted the plugins it has knowledge of (urdf, collada, and sdformat) by the position in the file that these strings occurred <robot, <collada, and <sdf, then if <sdf occurs before <robot then the sdformat parser will be tried before the urdf parser. This avoids unnecessary console spam in these three cases, while also enabling other robot description plugins (srdf?).

How about we prove the concept in existing distros with the heuristic above, then implement it using a new virtual function in the future ones? I think that order is good because the heuristic might still have a place with the new virtual function by avoiding the loading of unused plugins.

[clalancette]

isThisXMLForMe() would instead solve the console spam problem by allowing plugins to be tested without calling parse().

This isn't exactly how I was thinking of isThisXMLForMe() operating. What I was really thinking was that each plugin would implement the isThisXMLForMe() method. Then the high-level urdf::Model::initString() method would load the available plugins, and call isThisXMLForMe() on each of them until it found an acceptable match.

That being said, I'm not at all tied to that idea, I just want to make sure we are talking about the same thing.

Say urdf used the strategy of trying each parser until one worked, but it used a heuristic to pick the order that it would try. If it sorted the plugins it has knowledge of (urdf, collada, and sdformat) by the position in the file that these strings occurred <robot, <collada, and <sdf, then if <sdf occurs before <robot then the sdformat parser will be tried before the urdf parser. This avoids unnecessary console spam in these three cases, while also enabling other robot description plugins (srdf?).

I understand the desire (particularly about not loading unused plugins), but I was also trying to see if we could get URDF out of the heuristic game. In particular, if someone wanted to come along and add support for another format, it would be nice if they could do so without changing the urdf package at all. Just write code to parse your format, fill out the required API (including isThisXMLForMe()), and register the plugin. Then you have instant new format. It doesn't even have to be XML at that point, as long as they can fill out the urdf::ModelInterface structure.

How about we prove the concept in existing distros with the heuristic above, then implement it using a new virtual function in the future ones? I think that order is good because the heuristic might still have a place with the new virtual function by avoiding the loading of unused plugins.

I do think this is kind of backwards from our usual development process. In general I like to see if we can do this the right way (for some definition of "right"), regardless of API/ABI problems. We can land that for new distributions. Then I think it becomes clearer how to do it in older distributions (even if we have to have some hacks to preserve API/ABI).

[sloretz]

urdf::Model::initString() method would load the available plugins, and call isThisXMLForMe() on each of them until it found an acceptable match.

That being said, I'm not at all tied to that idea, I just want to make sure we are talking about the same thing.

I think we're talking about the same thing. Given some string, loop over all the plugins to see if they can handle it. If we got rid of the heuristic with only the current API it would mean the loop would have to call parse() and see if it returned a valid object. Calling parse() would result in console spam from plugins that don't support this string. Adding a new API isThisXMLForMe() would solve the console spam because those checks would be silent during the loop.

I was also trying to see if we could get URDF out of the heuristic game. In particular, if someone wanted to come along and add support for another format, it would be nice if they could do so without changing the urdf package at all

Having a heuristic in urdf doesn't have to block custom formats. I think it's a lack of a loop over all plugins that blocks new formats right now. Without a heuristic in urdf, adding a loop with only the current urdf_parser_plugin API would result in console spam. IsThisXMLForMe() solves that, but we can't add an API to urdf_parser_plugin (edit: in released distros) because it would break ABI. That's where a heuristic is valuable; urdf can avoid console spam in most cases by guessing which plugin is most likely to be able to handle the format and try that one first.

[gavanderhoorn]

Somewhat off-topic perhaps, but if we're going to add the discussed loop, please don't use IsThisXMLForMe as the name of the function to call: I might want to use .yaml for my robot model ;)

[clalancette]

Somewhat off-topic perhaps, but if we're going to add the discussed loop, please don't use IsThisXMLForMe as the name of the function to call: I might want to use .yaml for my robot model ;)

Yeah, you are totally right. I threw that out mostly as a joke (that's why I put a question mark in it). I expect whoever writes this for real will make up a better name 😄

[ros-discourse]

This issue has been mentioned on ROS Discourse. There might be relevant details there:

https://discourse.ros.org/t/web-based-urdf-visualization-tool-and-library-opensourced-from-nasa-jpl/14404/3

[davetcoleman]

My 2 cents: assuming @IanTheEngineer is willing to do this with his own resources, I don't see any problem with it. I agree with @Levi-Armstrong here are larger issues with URDF and its limitations. But I don't see how that affects this feature addition, beyond the argument that there will just be "more code to port" later.

Currently if you want to use SDF with other ROS tools, you have to use SDF's standalone converter to convert it to a URDF, then load it into the rest of the ROS pipeline. This new feature will streamline that greatly.

[ros-discourse]

This issue has been mentioned on ROS Discourse. There might be relevant details there:

https://discourse.ros.org/t/web-based-urdf-visualization-tool-and-library-opensourced-from-nasa-jpl/14404/4

[ros-discourse]

This issue has been mentioned on ROS Discourse. There might be relevant details there:

https://discourse.ros.org/t/ros-2-tsc-meeting-minutes-2020-07-16/15468/1

[sloretz]

Seeking feedback: ros2#13 adds an API might_handle() that asks parser plugins how likely it is they can parse the given data. It's used by this new parser plugin for SDFormat ros/sdformat_urdf#1

[sloretz]

@IanTheEngineer mind if I close this ticket?

This proposal has been implemented and released to ROS Rolling. The code is available here:
https://github.com/ros/sdformat_urdf/

To try it out, enable the ROS 2 testing repo on Ubuntu Focal, install ros-rolling-sdformat-urdf, create an SDFormat robot description with a few caveats, and launch robot_state_publisher plus something to publish joint states. There isn't joint_state_publisher support yet, so if you just want to run robot_state_publisher and RViz (without a simulator or real robot outputting real joint states) then you'll need to manually publish them.

There's an example of using it here: https://github.com/sloretz/drake_ros2_demos/

[IanTheEngineer]

I think we can close this ticket, well done! The SDFormat model limitations seem reasonable as a first-pass. I think we would need to extend urdf::Model to utilize additional features of SDFormat in the future. That, however, is certainly a separate ticket.