Fixed application of <sensor><pose> tags in lumped links during URDF conversion

[peci1]

🦟 Bug fix

Fixes #67 and #378.

Summary

There is a TODO in

https://github.com/osrf/sdformat/blob/8496164cac259e13083023efb06b76a04b1f8d3e/src/parser_urdf.cc#L3374-L3375

which states that <sensor><pose> tags are being silently ignored. This is the culprit of bugs #67 and #378. And of many headaches people spend converting their sensors, finding out after hours that they have to attach them to non-lumped links without any offset for the conversion to work.

This PR fixes the behavior and adds correct processing of <sensor><pose> and <projector><pose> for lumped links.

In addition to ignoring the <sensor><pose> values, there is also weird behavior. First,

https://github.com/osrf/sdformat/blob/8496164cac259e13083023efb06b76a04b1f8d3e/src/parser_urdf.cc#L3378

thinks it deletes the user-specified <sensor><pose> tag.

Second,

https://github.com/osrf/sdformat/blob/8496164cac259e13083023efb06b76a04b1f8d3e/src/parser_urdf.cc#L3406

thinks it adds the computed transform to <sensor><pose>.

The problem is that both of these lines of code work one level higher than they should. (*_blobIt) is a XML snippet containing the <sensor> tag, so there is no direct child <pose> of (*_blobIt). The second line then adds a new element <pose>, but again, as a sibling of <sensor>, not as a child. This is interesting and might have had unforseen consequences (as multiple pose tags are allowed, at least syntactically). Fortunately, there is a guard at

https://github.com/osrf/sdformat/blob/8496164cac259e13083023efb06b76a04b1f8d3e/src/parser_urdf.cc#L2128

that only allows the first child of (*_blobIt) to be passed further. This might look like another bug, but I actually utilized this behavior sealing it for the future (it has been this way for a decade, so...).

Currently, when there is no <sensor><pose> specified, the conversion process doesn't emit any <sensor><pose> in the end, even though it should accumulate something and I see it adding and removing the XML fragments. That is because of the fact it adds the <pose> at wrong level, and in the next level, it deletes/re-adds this sibling of <sensor>. With <sensor><pose> manually specified, it makes it appear in the result (but without the accumulated transform). That's because the DeleteChild function has no effect on <sensor><pose> but deletes the sibling <pose> instead (TinyXML2 doesn't fail when deleting a wrong child).

The fix I propose in this PR is somewhat complicated to grasp on. First idea of just storing _reductionTransform * sensorPose in the <sensor><pose> field does not work as all but the first lump transforms get doubled. What I do is I take the <sensor><pose> when first encountering it and store the <pose> as a sibling of <sensor>. As said earlier, this sibling will not bubble up anywhere, but it is the scratchpad that is needed. <sensor><pose> is then used for storing the actual _reductionTransform * sensorPose. What is important is that the original <sensor><pose> is not updated and we always have access to it in the original form, so that we can take the recursively computed reduction transform and add the sensor pose exactly once.

I was also looking for a nicer way to parse the 6-tuple transform string, but haven't found any usable part of the library. If you know how e.g. sdf::Param could be used to simplify and reuse-ify the code, let me know.

I also added a comprehensive test suite based on both mentioned issues and my experiments. I also verified the change running ign sdf -p on https://github.com/osrf/subt/tree/master/submitted_models/ctu_cras_norlab_absolem_sensor_config_1 model which might go as deep as 10 levels of joints/links (or maybe even more) with sensors at the very tips. I tried exchanging the last link transform for a sensor pose and the result was the same.

---

As a workaround until this PR is merged, this is what can be done.

1. Attach the sensor to a non-lumped link and make the origins of the link and the sensor identical. Non-lumped links are set by <preserveFixedJoint>true</preserveFixedJoint>
2. If you want to have the sensor on a lumped link, you have to manually compute the transformation of the sensor to its nearest non-lumped link. Once you have this transformation, add it in <sensor><pose>. Even though it seems you're duplicating the tree-induced transform by the <pose> tag, you are not because of the reported bugs.

---

Interestingly, the part of code I fixed was mostly untouched for 8 years. However, it seems that gz sdf -p from Gazebo 9 behaves differently.

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed) (how should I do it before submitting the PR when there is need to write down the PR number I don't yet have?)
• Code check passed (In source directory, run sh tools/code_check.sh)
• All tests passed (See
  test coverage)
• While waiting for a review on your PR, please help review
  another open pull request
  to support the maintainers

Note to maintainers: Remember to use Squash-Merge

[codecov-io]

Codecov Report

Merging #525 (8496164) into sdf10 (436b28d) will decrease coverage by 0.09%.
The diff coverage is 86.57%.

❗ Current head 8496164 differs from pull request most recent head d8f92de. Consider uploading reports for the commit d8f92de to get more accurate results

@@            Coverage Diff             @@
##            sdf10     #525      +/-   ##
==========================================
- Coverage   87.75%   87.65%   -0.10%     
==========================================
  Files          62       71       +9     
  Lines        9594    10104     +510     
==========================================
+ Hits         8419     8857     +438     
- Misses       1175     1247      +72     

Impacted Files	Coverage Δ
include/sdf/Element.hh	100.00% <ø> (ø)
include/sdf/Filesystem.hh	100.00% <ø> (ø)
include/sdf/Types.hh	100.00% <ø> (ø)
src/SDF.cc	93.33% <ø> (+0.98%)	⬆️
src/Scene.cc	100.00% <ø> (ø)
src/ScopedGraph.hh	98.86% <ø> (ø)
src/SemanticPose.cc	100.00% <ø> (ø)
src/Sensor.cc	80.18% <ø> (-1.90%)	⬇️
src/Sky.cc	100.00% <ø> (ø)
src/Sphere.cc	90.00% <ø> (-3.62%)	⬇️
... and 86 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 436b28d...d8f92de. Read the comment docs.

[nkoenig]

Looks good to me. Thanks for the tests.

[nkoenig]

Minor CI fix needed.

[doisyg]

Is it possible to merge also in edifice/sdf11 ? (as it should fix gazebosim/ros_gz#147)

[chapulina]

Is it possible to merge also in edifice/sdf11 ?

Being forward-ported on #539