CostmapModel类继承了WorldModel，能够获取点、连线、多边形轮廓的cost，是局部规划器与costmap间的一个桥梁。初始化在move_base的构造函数，一般如下使用：

//costmap_2d::Costmap2DROS*  planner_costmap_ros_
world_model_ = new base_local_planner::CostmapModel(*(planner_costmap_ros_ ->getCostmap()) ); 
std::vector<geometry_msgs::Point> footprint = planner_costmap_ros_->getRobotFootprint();
double footprint_cost = world_model_->footprintCost(x_i, y_i, theta_i, footprint);

CostmapModel类帮助local planner在Costmap上进行计算, footprintCost, lineCost, pointCost三个类方法分别能通过Costmap计算出机器人足迹范围、两个cell连线、单个cell的代价，并将值返回给局部规划器。

footprintCost

原型

double base_local_planner::CostmapModel::footprintCost(const geometry_msgs::Point &  position,
      const std::vector< geometry_msgs::Point > &   footprint,
      double  inscribed_radius,
      double  circumscribed_radius 
)

Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid.

参数:

• position The position of the robot in world coordinates
• footprint The specification of the footprint of the robot in world coordinates
• inscribed_radius The radius of the inscribed circle of the robot
• circumscribed_radius The radius of the circumscribed circle of the robot

返回值:
Positive if all the points lie outside the footprint, negative otherwise: -1 if footprint covers at least a lethal obstacle cell, or -2 if footprint covers at least a no-information cell, or -3 if footprint is [partially] outside of the map

footprintCost函数，先调用WorldModel类里面的footprintCost函数，用x,y,th把footprint_spec变成世界坐标下坐标，还得到原点在世界坐标系坐标，内切半径，外切半径。根据这些，调用CostmapModel::footprintCost函数，转换到costmap坐标系下，用光栅化算法得到栅格点，由栅格点的代价值得到足迹点线段的代价值，再得到整个足迹点集的代价值就可以了。

footprintCost函数为提高效率，它不计算足迹所有栅格的代价，认为只要计算N条边涉及到的栅格，就可计算出最大代价。首先尝试获取线段的地图坐标，然后利用lineCost函数计算线段代价值，并用footprint_cost参数保存最大的线段代价值，最终如果正常，则返回的也是footprint_cost。只要有1个线段的代价值小于0，则直接返回-1。

注意函数和栅格计算的代价值不同：返回值中，最大值是253， 即INSCRIBED_INFLATED_OBSTACLE，因为比253大的 LETHAL_OBSTACLE (254), NO_INFORMATION (255)会分别返回-1、-2，表示不可行。

返回值:

• -1.0 ：覆盖至少一个障碍cell
• -2.0 ：覆盖至少一个未知cell
• -3.0 ：不在地图上
• 其他: a positive value for traversable space

没有撞障碍时，返回值是 0

函数原型对第一个参数position的解释是：The position of the robot in world coordinates。实际是 局部代价地图的全局坐标系 ，一般是 map 或 odom

使用

以tf::Stamped<tf::Pose>为输入类型

tf::Stamped<tf::Pose> input_pose;
input_pose.setOrigin(tf::Vector3(x, y, 0) );
input_pose.setRotation(tf::createQuaternionFromYaw(theta) );
input_pose.frame_id_ = "map";
input_pose.stamp_ = ros::Time::now();

tf::Stamped<tf::Pose> global_pose_in_local_costmap;
auto tf = context_->tf_;
try
{
    tf->transformPose( local_planner_costmap_ros_->getGlobalFrameID(), 
                  input_pose,
                  global_pose_in_local_costmap);
}
catch (tf::TransformException& ex)
{
    ROS_INFO("Cannot transform in inCollision !");
    return false;
}
double temp_x = global_pose_in_local_costmap.getOrigin().getX();
double temp_y = global_pose_in_local_costmap.getOrigin().getY();

double footprint_cost = world_model_->footprintCost(
    temp_x,
    temp_y,
    tf::getYaw(global_pose_in_local_costmap.getRotation() ),
    local_costmap_ros->getRobotFootprint() );

函数中的内外接圆半径实际没有用，对于多边形轮廓，是否需要修改？

源码

double CostmapModel::footprintCost(const geometry_msgs::Point& position, 
    const std::vector<geometry_msgs::Point>&  footprint,
    double inscribed_radius,  double circumscribed_radius )
{
    //used to put things into grid coordinates
    unsigned int cell_x, cell_y;
    //get the cell coord of the center point of the robot
    if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
      return -3.0;

    //省略  footprint的点少于3个，只取点位置的代价
    if(footprint.size() < 3)
    { ...... }
    // 把足迹视为多边形，循环调用 lineCost 计算多边形各边的cell，注意首尾闭合，最后返回代价
    // 对于圆形底盘，按16边形处理
    // now we really have to lay down the footprint in the costmap grid
    unsigned int x0, x1, y0, y1;
    double line_cost = 0.0;
    double footprint_cost = 0.0;
    // we need to rasterize each line in the footprint
    for(unsigned int i = 0; i < footprint.size() - 1; ++i)
    {
       //get the cell coord of the first point
      if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
        return -3.0;

      //get the cell coord of the second point
      if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
          return -3.0;

      line_cost = lineCost(x0, x1, y0, y1);
      footprint_cost = std::max(line_cost, footprint_cost);
      //if there is an obstacle that hits the line, return false
      if(line_cost < 0)
          return line_cost;
    }
    // 以下是连接 footprint 的第一个和最后一个点
    //get the cell coord of the last point
    if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
        return -3.0;

    //get the cell coord of the first point
    if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
        return -3.0;

    line_cost = lineCost(x0, x1, y0, y1);
    footprint_cost = std::max(line_cost, footprint_cost);

    if(line_cost < 0)
        return line_cost;

    // if all line costs are legal, return that footprint is legal
    return footprint_cost;
  }

搜索发现有3个lineCost函数, TrajectoryPlanner::lineCost, VoxelGridModel::lineCost, CostmapModel::lineCost。经过运行，发现调用的还是CostmapModel::lineCost

对于线段的代价计算函数lineCost，其实是对线段中的所有像素点（通过bresenham算法得到，不必深入研究）进行代价计算，如果像素点的代价为LETHAL_OBSTACLE或者NO_INFORMATION，则该点代价为-1。如果某个点的代价为-1，则该线段的代价也为-1，否则取所有点中最大的代价作为线段的代价

// return a positive cost for a legal line 或者负值
double CostmapModel::lineCost(int x0, int x1, int y0, int y1) const
{
    double line_cost = 0.0;
    double point_cost = -1.0;
    for( LineIterator line( x0, y0, x1, y1 ); line.isValid(); line.advance() )
    {
      point_cost = pointCost( line.getX(), line.getY() );
      if(point_cost < 0)
        return point_cost;

      if(line_cost < point_cost)
        line_cost = point_cost;
    }
    return line_cost;
}

double CostmapModel::pointCost(int x, int y) const
{
    unsigned char cost = costmap_.getCost(x, y);
    //if the cell is in an obstacle the path is invalid
    if(cost == NO_INFORMATION)
      return -2;
    if(cost == LETHAL_OBSTACLE)
      return -1;

    return cost;
}

参考：
CostmapModel::footprintCost
ObstacleCostFunction类，继承自Trajectory类