The CollisionRobotFCL::distanceSelf returns incorrect distance with urdf's that have multiple geometry in a single link.

[Levi-Armstrong]

The distanceCallback function in collision_common.cpp needs to check to see if both geometries are associated to the same link. If the below code is add after the "If active components are specified" check along with l1 and l2 being instantiated outside the if statement it solves the issue.

// Check if it is an internal link geometry check
if (l1->getName() == l2->getName())
{
  return false;
}

[mikeferguson]

seems that we have merged fixes last year