Improve contact sensor / visualization performance

[iche033]

Signed-off-by: Ian Chen [email protected]

🦟 Bug fix

Summary

This PR improves contact sensor performance by using a new GetByPhysicsId function in EntityFeatureMap for faster retrieval of gazebo entities from physics ID.

The original call uses the Get function that requires casting to ShapePtrType which causes a big performance hit when done many times in a for loop. The cast is needed because the two types are actually different.

With this change, I noticed that for our application with ~70 contact points, the computation time of the for loop for building the contact map drops from 2.x ms to 0.02x ms. This gives us ~10% RTF Improvement with a contact sensor in the world.

You can manually test this with shapes.world:

1. Launch shapes.sdf world
2. spawn 5 more cylinders into the world using the GUI tool: this gives ~60 contact points.
3. Go to top right 3 dots menu and open the Visualize Contacts GUI Plugin

Before this change, the RTF on my machine is roughly 3x%. After this change, it hovers around 8x%

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed)
• codecheck passed (See contributing)
• 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 and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[codecov]

Codecov Report

Merging #1452 (60c88ad) into ign-gazebo6 (df62658) will not change coverage.
The diff coverage is n/a.

@@             Coverage Diff              @@
##           ign-gazebo6    #1452   +/-   ##
============================================
  Coverage        33.58%   33.58%           
============================================
  Files               44       44           
  Lines             2260     2260           
============================================
  Hits               759      759           
  Misses            1501     1501           

---

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 df62658...60c88ad. Read the comment docs.

[ahcorde]

Nice!