TEB算法中的footprint模型是单独构建的，而没有直接调用通用代价地图中的 footprint，二者footprint模型不同，但在TEB中二者都有使用。

TEB的footprint用于图优化，costmap的 footprint用于 `feasibility check`环节

LayeredCostmap类的成员函数： double getInscribedRadius() { return inscribed_radius_; }

对于polygon轮廓，两种footprint模型的内接半径相同， TEB的模型没有返回外接半径的函数

在TebLocalPlannerROS::initialize中定义: RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh);，然后进行模型验证validateFootprints

在planner初始化时，也就是在TebOptimalPlanner::initialize中赋值给robot_model_.

RobotFootprintModelPtr 其实就是 BaseRobotFootprintModel类型的 boost::shared_ptr

很显然BaseRobotFootprintModel是个抽象类，定义了机器人footprint/contour models的接口，根据yaml设置的类型而定义相应的类，这个类只用于optimization，代价地图的footprint只用于检查路径的可行性

• Point 和 circular模型的footprints要求算力最少。

• two-circles模型要求的是两次 distance calculations

• line模型增加了一些 simple case checking, 但是算力在大多数情况仍要求不高。

• polygon模型对每一个edge都要求 distance check，所以计算时间主要取决于 edges和vertices 的数量

也就是说这几种模型的算力要求依次增大。

RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh)
{
  std::string model_name; 
  if (!nh.getParam("footprint_model/type", model_name))
  {
	// 如果未指定类型，则使用点类型
    return boost::make_shared<PointRobotFootprint>();
  }
  // point  
  if (model_name.compare("point") == 0)
  {
      ROS_INFO("Footprint model 'point' loaded for trajectory optimization.");
      return  boost::make_shared<PointRobotFootprint>();
  }
  // circular   省略
  if (model_name.compare("circular") == 0)
  // line  省略
  if (model_name.compare("line") == 0)
  // two circles 省略
  if (model_name.compare("two_circles") == 0)

  // polygon  只对 Point2dContainer vertices_; 赋值
  if (model_name.compare("polygon") == 0)
  {
    // check parameters
    XmlRpc::XmlRpcValue footprint_xmlrpc;
    if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc) )
    {
        ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for 
      	trajectory optimization, since param '" << nh.getNamespace() << 
      	"/footprint_model/vertices' does not exist. Using point-model instead");
        return boost::make_shared<PointRobotFootprint>();
    }
    // get vertices
    if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
    {
      try
      {
        Point2dContainer polygon = makeFootprintFromXMLRPC(footprint_xmlrpc, 
        	"/footprint_model/vertices");
        ROS_INFO_STREAM("Footprint model 'polygon' loaded for trajectory optimization.");
        return boost::make_shared<PolygonRobotFootprint>(polygon);
      } 
      catch(const std::exception& ex)
      {
        ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization: "
        	<< ex.what() << ". Using point-model instead.");
        return boost::make_shared<PointRobotFootprint>();
      }
    }
    else
    {
      ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, 
      	since param '" << nh.getNamespace() << "/footprint_model/vertices' does not 
      	define an array of coordinates. Using point-model instead.");
      return boost::make_shared<PointRobotFootprint>();
    }
  }
  return boost::make_shared<PointRobotFootprint>();
}

validateFootprints

// TebLocalPlannerROS::initialize中的调用
validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, 
          cfg_.obstacles.min_obstacle_dist);

void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, 
    double costmap_inscribed_radius, double min_obst_dist)
{}

函数就一句ROS_WARN_COND，要求 TEB优化所用的机器人内接半径 + 参数 min_obstacle_dist > costmap中的内接半径(包括footprint_padding) ，否则Infeasible optimziation results会频繁出现

这里就是设置为点类型的缺陷之处，第1参数个会是0，但是如果min_obstacle_dist超过costmap中的内接半径，会无法走窄通道。

BaseRobotFootprintModel的派生类实现了calculateDistance函数，计算机器人到障碍物的 Euclidean距离，用于图优化环节。以最简单的Point类型为例

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

至于细节，需要单独一篇文章分析

变量robot_model_在之后用于图优化时的一系列函数：
AddEdgesObstacles
AddEdgesObstaclesLegacy
AddEdgesDynamicObstacles
AddEdgesVelocityObstacleRatio，也就是所有和障碍相关的约束

以及可视化中的publishInfeasibleRobotPose