在TEB中的footprint模型 中提到calculateDistance函数计算机器人到障碍物的 Euclidean 距离，用于图优化环节。

以最简单的Point类型为例

virtual double calculateDistance(const PoseSE2& current_pose, 
    const Obstacle* obstacle) const
{
	return obstacle->getMinimumDistance(current_pose.position());
}

Obstacle是个抽象类，它的派生类有

PointObstacle
CircularObstacle
PillObstacle
LineObstacle
PolygonObstacle

显然不同的障碍类型对函数有不同实现，这里又涉及到costmap_converter和Homotopy，这就更复杂了

createGraph() —— PointObstacle::checkLineIntersection —— PointObstacle::checkCollision