Fix Condition for Adding current DistanceResultData to DistanceMap for DistanceRequestType::SINGLE

[jonasTUM]

Description

One line of code is changed in the collision calculation interface file for FCL.

Within the settings for a distance request within MoveIt! the user can specify a DistanceRequestType.
I am using distance requests for avoiding collisions of the robot. Therefore, I want to check every collision geometry of a single robot link with the world objects. I am only interested in the closest point of each robot link to the world object, i.e. in the collision geometry of the robot link that is closest to the world object. Therefore, I am using DistanceRequestType::SINGLE. However, when using SINGLE MoveIt! does not store the results for the closest collision object in the DistanceMap of the DistanceResult. Instead, MoveIt! stores the distance result for a random collision geometry of the robot link in the DistanceMap, and not the results for the closest collision geometry. This results in the assumption that a certain robot link is far away from an obstacle although collision is very close.

I think, this is due to the wrong condition for replacing the current DistanceResultData in the DistanceMap in case of SINGLE distance request types. If the distance of the current collision geometry is smaller than the stored one, the stored one should be replaced.

In my case, I got a more reasonable robot behavior after having applied this change.

[welcome]

Thanks for helping in improving MoveIt and open source robotics!

[codecov-io]

Codecov Report

Merging #1963 into master will increase coverage by 0.00%.
The diff coverage is 0.00%.

@@           Coverage Diff           @@
##           master    #1963   +/-   ##
=======================================
  Coverage   49.88%   49.88%           
=======================================
  Files         315      315           
  Lines       24739    24739           
=======================================
+ Hits        12340    12341    +1     
+ Misses      12399    12398    -1     

Impacted Files	Coverage Δ
...e/collision_detection_fcl/src/collision_common.cpp	73.23% <0.00%> (ø)
...nning_scene_monitor/src/planning_scene_monitor.cpp	55.98% <0.00%> (+0.14%)	⬆️

---

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 eb839d3...918a4e9. Read the comment docs.

[v4hn]

Hello @jonasTUM ,

Wow, that's quite an involved error you found there.
Thank you for the analysis and contributing a patch 👍
Logically it makes a lot of sense to me.

I did not spend too much time on this, but tried to come up with a test case for this yesterday.
Aside from the fact that it triggers for the bullet plugin on my system, it does not trigger for your issue.
Could you have a look at it and find out how my request differs from your case?

[jonasTUM]

Hello @v4hn,

actually, I am not so familiar with testing. I have seen that within the moveit_core package I can build all the test files with catkin run_tests --no-deps --this -iv. However, this takes some time and always also runs all the tests if I am right.

• Is there another way that allows to only build and run a specific test target, e.g. the test_fcl_collision_detection_panda? Or at least to only build (not run) all tests in the package?
• Do the tests allow outputs to the console?
• I could not reproduce the error that should be caught in case of bullet. After having evaluated the tests via catkin_test_results build/test_results there was nothing like test_bullet_collision_detection_panda failed ... or whatever I should expect. How can I see whether a specific test succeeded?

I am wondering, whether testing a distance request type makes sense for the bullet library. I think that the distance request type, e.g. collision_detection::DistanceRequestTypes::SINGLE, is only used in case of the /moveit_core/collision_detection_fcl/src/collision_common.cpp and not in case of the /collision_detection_bullet/src/ files.
I will look at the test you wrote within the next days an try to get reasonable output for FCL (after you told me how to check for the success of the test_fcl_collision_detection_panda 😉).

[v4hn]

For building the test, I would just rebuild the package, as cmake will only rebuild changed files.

... [how to run the specific test]

https://answers.ros.org/question/271114/catkin-run-specific-unit-test/

Using an install workspace with catkin_tools, I can run the specific test via

./devel/.private/moveit_core/lib/moveit_core/test_fcl_collision_detection_panda --gtest_filter=*Single

This call will also print the regular output of the tests.

the distance request type, e.g., SINGLE, is only used in case of fcl and not in case of the bullet files.

True, someone implemented Distance calls as print "not implemented" 😄

[v4hn]

The new test catches the issue now.
It's a bit fiddly to get the broadphase checks to reorder the collision objects such that the line you change ever executes, but I got there now using cylinders and random poses...

I'll merge once the tests succeeds here.

[v4hn]

The new test fails on current upstream and succeeds with this patch.

Thank you for the contribution @jonasTUM !

[welcome]

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