collision_detection_fcl: Report link_names in correct order

[xqms]

Description

For distance queries, FCL may report results in different order than what was requested (e.g. fcl::distance(o1, o2, ...) might give an fcl::DistanceResult with o2, o1. The MoveIt wrapper does not check if this is the case, and thus may report nearest_points in order o2,o1 with link_name in order o1,o2.

This PR fixes this issue by explicitly using the collision objects referenced in the fcl::DistanceResult object. It also adds a unit test which demonstrates the issue.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
  I feel this is not applicable, since this is a pure bugfix.
• Document API changes relevant to the user in the MIGRATION.md notes
  I think the risk that somebody depends on the current broken behavior is quite low.
• Create tests, which fail without this PR reference

[welcome]

Thanks for helping in improving MoveIt and open source robotics!

[v4hn]

Thank you for the contribution and greetings to Bonn!

It's unexpected to see that FCL can change the order here and I wonder whether they introduced this issue in some later release.
I'm happy to consider the case in MoveIt, but I would also expect a bug report at the FCL repository. Did you create one there as well?

It would be great if you could improve the test case according to my comment below. Thank you lots for creating a test case to begin with. 👍

[xqms]

@v4hn thanks for the quick feedback and greetings back to Hamburg ;)

I'll improve the test case tomorrow or Monday and will look at the CI failure as well. I just wanted to write down what I found out about FCL itself:

Here is the code that applies the distance algorithm "in reverse" depending on the object type:
https://github.com/flexible-collision-library/fcl/blob/809dc40cad5a9d925214e1d1089f0e48193de413/include/fcl/narrowphase/distance-inl.h#L117-L138

and here is the same piece of code 9 years ago (it got moved around a bit, but is still very much the same):
https://github.com/flexible-collision-library/fcl/blob/0572525a7206915870ccff2e7b5d550eb7867ab0/src/distance.cpp#L81-L102

So I think it's safe to say that this has been the case for quite some time and is, I guess, intentionally designed that way. I'll open an issue with FCL about documenting this more explicitly. Changing the behavior on FCL's side is probably not a good idea.

[AndyZe]

I'll give this a test today. Seems like a good idea, overall.

[v4hn]

Looks good to me now (unless CI objects).

Thank you for taking the time to clean up the test!
@AndyZe Please merge if your test succeeds.

[xqms]

CI is failing since FCL < 0.6 (melodic uses 0.5.0) reports the nearest_points sometimes in the wrong frame (body coordinates):

flexible-collision-library/fcl#171, flexible-collision-library/fcl#288

In that case, the nearest_points and normal fields are completely wrong and there is no easy way to fix them on our side. Should we disable the test for FCL < 0.6? It's not easy to do, since the tests are templated on the collision solver type 🤔

[xqms]

Should we disable the test for FCL < 0.6? It's not easy to do, since the tests are templated on the collision solver type

I went ahead and did that, so CI should pass now.

[v4hn]

Should we disable the test for FCL < 0.6?

Thanks for implementing it. I would propose to go one step further though!
If it's documented in github issues that this request produces inconsistent results with FCL < 0.6 we should simply reject the actual request from user code in this case and ROS_ERROR tell them to upgrade their FCL version if they want to use it.

[codecov]

Codecov Report

Merging #2682 (367c0b4) into master (b2b7e6d) will increase coverage by 0.02%.
The diff coverage is 68.43%.

@@            Coverage Diff             @@
##           master    #2682      +/-   ##
==========================================
+ Coverage   60.53%   60.55%   +0.02%     
==========================================
  Files         402      402              
  Lines       29618    29595      -23     
==========================================
- Hits        17926    17917       -9     
+ Misses      11692    11678      -14     

Impacted Files	Coverage Δ
...e/collision_detection_fcl/src/collision_common.cpp	77.92% <54.55%> (-0.43%)	⬇️
.../collision_detection_fcl/src/collision_env_fcl.cpp	89.64% <80.00%> (-0.25%)	⬇️
.../collision_detection/test_collision_common_panda.h	99.37% <100.00%> (+0.02%)	⬆️
...meterization/work_space/pose_model_state_space.cpp	81.42% <0.00%> (-1.92%)	⬇️
...raint_samplers/src/default_constraint_samplers.cpp	83.89% <0.00%> (-0.32%)	⬇️
...veit/occupancy_map_monitor/occupancy_map_updater.h	100.00% <0.00%> (ø)
...include/moveit/collision_detection/occupancy_map.h
...clude/moveit/occupancy_map_monitor/occupancy_map.h	25.00% <0.00%> (ø)
moveit_core/planning_scene/src/planning_scene.cpp	60.33% <0.00%> (+0.83%)	⬆️
... and 1 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 b2b7e6d...367c0b4. Read the comment docs.

[xqms]

Good idea! I implemented it as a throttled ROS_ERROR that allows the program to continue. That way, older programs that have enable_nearest_points=true and somehow are not affected by the points being switched don't break immediately.

[welcome]

Congrats on getting your first MoveIt pull request merged and improving open source robotics!

[v4hn]

Thanks @xqms . Please send more patches if you encounter other issues 🥇

[xqms]

Also thanks to you both @v4hn and @AndyZe for the fast review & feedback!

@v4hn Just wanted to point out that we did change the behavior for enable_signed_distance=true and enable_nearest_points=false now. Do you think we need to add something to MIGRATION.md?

To follow up on the performance discussion: I couldn't measure a difference between master (before the merge) and this PR. Here's the benchmark I used: https://gist.github.com/xqms/0500ed88ac9836f6bbe0022298635c8e
I consistently get 10ms per distanceSelf() call, nearly all of which is spent inside FCL according to the profiler. @AndyZe maybe your use case is different?