This addresses issue #258: When requesting distance information for urdf's that have links with multiple geometries, an incorrect distance is returned. This is a result of the distanceCallback function not checking if both geometries are associated to the same object as done in the collisionCallback.
This addresses issue #258: When requesting distance information for urdf's that have links with multiple geometries, an incorrect distance is returned. This is a result of the distanceCallback function not checking if both geometries are associated to the same object as done in the collisionCallback.