costmap_3d: query: abstract out the distance cache

Abstract out the distance caches into an internal distance cache class.
Move all the distance cache logic and methods into that internal class.
This change should make no functional difference, as it is simply
reoganizing the code.

A future change could be to further split the internal classes up into
separate classes and files for clarity. This is not yet done as it will
take even more reorganization to disentangle the caches from the
queries.

The main spirit of this commit is to remove all the places where
the code was repeating itself by getting all the details of the
various distance caches into one DistanceCache class.