URDF RPY/XYZ parsing

[BenBlumer]

There's a few sneaky URDF parsing issues that caught me:

1. in offsets.cpp: boost::split(rpy_pieces, rpy_str, boost::is_any_of(" ")); If the model doesn't contain an explicit RPY, this segfaults.

2. This is the real killer: the way the XYZ and RPY strings are parsed, if there's two spaces between elements, rpy_pieces.size() and/or xyz_pieces.size() equal 4. That silently refuses to apply transformations, but outputs a URDF as if everything is hunky dory. These are the relevant lines in offsets.cpp:

        if (xyz_pieces.size() == 3)
        {
          origin.p = KDL::Vector(boost::lexical_cast<double>(xyz_pieces[0]), boost::lexical_cast<double>(xyz_pieces[1]), boost::lexical_cast<double>(xyz_pieces[2]));
        }

        if (rpy_pieces.size() == 3)
        {
          origin.M = KDL::Rotation::RPY(boost::lexical_cast<double>(rpy_pieces[0]), boost::lexical_cast<double>(rpy_pieces[1]), boost::lexical_cast<double>(rpy_pieces[2]));
        }

These are examples of problematic URDFs: I'm not sure if these are to URDF "spec" or not, but they do pass check_urdf without complaint.

<?xml version="1.0" ?>
<robot name="borked_ur5">
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/base.dae"/>
      </geometry>
    </visual>
  </link>
</robot>

<?xml version="1.0" ?>
<robot name="borked_ur5">
  <link name="base_link">
    <visual>
      <origin xyz="0.0 0.0 0.0 3.14 3.14  3.14"/>
      <geometry>
        <mesh filename="package://ur_description/meshes/ur5/visual/base.dae"/>
      </geometry>
    </visual>
  </link>
</robot>

I'm happy to submit a PR to fix (1) and throw an exception on (2). Before doing so, I'd like to get your thoughts: is it better to address these things as they are, or implement one of the URDF parsing libraries like [urdfdom(http://wiki.ros.org/urdfdom_py)?

[mikeferguson]

Yes, PRs for this would definitely be accepted. Probably best to also add a test with example borked URDFs so we continue to handle that in the future if things get updated at any point.

For the extra spaces, I imagine it might also be possible to drop elements from the xyz_pieces vector if they are empty strings? Might save some people time by not having an exception to track down.

If I recall correctly, the reason we didn't use urdfdom when originally implementing this was that certain un-parsed elements would get dropped (this was especially true of things like gazebo add-ons that aren't part of the "official" spec of URDF, but widely used). This was initially implemented a decade ago though, so it's possible that has improved.