MapGridCostFunction

• 当目标是global_plan中goal点时(is_local_goal_function_=true)。要计算点A到零线点goal的距离，就是从零线点开始，依次画两条直线。第一条垂直，到达A所在的y行，假设那个点是b；第二条水平，由b到A，这两条线经过的栅格数就是距离。于是理论上最大距离就是“size_x_ + size_y_”，对60x60代价地图，最大值是120。所在点直观看就是对角线上的两个点，实际中(local_goal_x, local_goal_y)不大可能恰好是(0, 0)或(59, 0)，也就不大可能到最大值。
• 当目标是global_plan这条路径本身时(is_local_goal_function_=false)。要计算点A到零线global_plan的距离，就是在global_plan找出一个离A最近的路径点b，这个b往往就是从A点画一条水平线或垂线，这条线和global_plan交点就是b。由于零线global_plan是一条线，含有多个点，最大距离通常会比目标是Goal点时的小。

MapGridCostFunction的成员变量

• std::vector target_poses_。planner_util_.getLocalPlan算出的transformed，即局限在local_costmap代价地图内、并且odom坐标系下的global_path。

path_costs_，goal_costs，goal_front_costs_，alignment_costs_四条准则都是MapGridCostFunction类型，因此打分思路都一致，只是通过设置不同参数即可实现不同的打分方式。

让以aggregationType_都是Last，goal_front_costs_和alignment_costs_的xshift_是0.3，直观理解4条准则是如何计算cost。要注意，机器人中心在map坐标系下坐标不一定是(30, 30)，往往是个比30小的值，像(28, 29)。

xshift_(0.3)。把轨迹A前移0.3米生成另一条轨迹，这两条轨迹有一样的路径点数目，对应的路径点有一样的th_pts_。轨迹A最后一个路径点A.g(34, 33)，在新轨迹变成了Ashift.g(33, 39)。在一些设置中，global_plan的Goal点也要前移，变量forward_point_distance_指示前移距离，值等于xshift_。在那些场景，goal_front_costs_的目标点要换成那个前移后的Goal。

aggregationType_(Last)。值是Last时，轨迹中最后一个路径点到目标的距离就是最终cost。

is_local_goal_function_。用于决定目标是global_plan中的Goal点，还是global_plan这条路径本身。

1. goal_front_costs_。使用前移后轨迹Ashift，目标是global_plan中的Goal点。AShift.g(33, 39)到Goal(45,19)距离将做为最终cost。如何计算这距离？直观上可这么算：从零线点Goal(45,19)开始，依次画两条直线。第一条垂直，到达AShift.g所在的y行(45, 39)，这时距离是20；第二条水平，由(45,39)到AShift.g(33,39)，距离12，这两距离和32就是这条准则算出的cost。
2. alignment_costs_。使用前移后轨迹Ashift，目标是global_plan这条路径本身。AShift.g(33, 39)到global_plan这条线的距离将做为最终cost。如何计算这距离？在global_plan找出一个离AShift.g最近的路径点G.Ashift，然后计算AShift.g到G.Ashift距离。示例中，就是从Ashift.g画一条到global_plan垂线，交点是G.Ashift，之间距离是13。
3. path_costs_。使用原轨迹A，目标是global_plan这条路径本身。A.g(34, 33)到global_plan这条线的距离将做为最终cost。类似alignment_costs_，在global_plan找出一个离A.g最近的路径点G.A，然后计算A.g到G.A距离。示例中，就是从A.g画一条到global_plan垂线，交点是G.A，之间距离是7。
4. goal_costs_。使用原轨迹A，目标是global_plan中的Goal点。A.g(34, 33)到Goal(45,19)距离将做为最终cost。类似goal_front_costs_，从零线点Goal(45,19)开始，依次画两条直线。第一条垂直，到达A.g所在的y行(45, 33)，这时距离是14；第二条水平，由(45,33)到A.g(34,33)，距离11，这两距离和25就是这条准则算出的cost。

一、MapGridCostFunction::prepare

在每个findBestPath周期时，会对各个打分项进行prepare操作，其中oscillation_costs_和obstacle_costs_的prepare不执行具体动作，而和MapGridCostFunction有关的四个打分项在prepare中会对局部地图进行预计算，从而方便后边进行打分。

预计算的思想在于维护一个跟局部地图尺寸对应的MapGrid地图（MapGrid类型），该地图中的每个栅格为MapCell类型，该类型记录了该栅格距离目标点或距离路径的距离信息。在每次打分前，先利用局部路径(target_poses_)更新该地图信息，并根据is_local_goal_function参数决定记录距离目标点的距离信息还是距离路径的距离信息，这些信息在后面打分时可以直接使用。

bool MapGridCostFunction::prepare() {
  map_.resetPathDist();

复位整个MapGrid地图

  if (is_local_goal_function_) {
    map_.setLocalGoal(*costmap_, target_poses_);
  } else {
    map_.setTargetCells(*costmap_, target_poses_);
  }

根据is_local_goal_function_，决定是采用setLocalGoal(...)还是setTargetCells(...)计算对应的预处理信息。

  return true;
}

对于setLocalGoal函数，是用来计算MapGrid地图中所有点到局部地图路径末端点的最短距离。在该函数中，首先对映射到局部地图中的路径进行栅格级的插补(MapGrid::adjustPlanResolution)，从而使得整个路径是栅格连续的。然后，查找路径在局部地图中的最后一个点，并将MapGrid地图中的该点标记为已访问（标记为已访问的点可以进行最短路径计算），然后调用computeTargetDistance函数采用类似dijkstra算法的逐步探索的方式，计算出MapGrid地图中所有点（栅格级）相对于与标记点的最短距离。

对于setTargetCells函数，是用来计算轨迹MapGrid地图中所有点到局部地图路径的最短距离。与setLocalGoal函数的区别主要在于，首先将局部地图路径中所有的点标记为已访问，然后调用computeTargetDistance函数就会计算MapGrid地图中所有点到整个局部地图路径的最短距离，具体算法需要进一步了解。

因此，在经过预计算后，对于path_costs_和alignment_costs这两个考虑与路径距离的打分项，其内部的MapGrid地图中存储的是所有点到路径的最短距离；对于goal_costs_和goal_front_costs_这两个考虑与局部目标点距离的打分项，其内部的MapGrid地图中存储的是所有点到局部目标点的最短距离。

至此，完成了path，goal相关的打分项的预计算，下一步就是利用scoreTrajectory(Trajectory &traj)进行具体的打分。

二、MapGridCostFunction::scoreTrajectory

double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0.0;
  if (aggregationType_ == Product) {
    cost = 1.0;
  }
  double px, py, pth;
  unsigned int cell_x, cell_y;
  double grid_dist;

  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);

    // translate point forward if specified
    if (xshift_ != 0.0) {
      px = px + xshift_ * cos(pth);
      py = py + xshift_ * sin(pth);
    }
    // translate point sideways if specified
    if (yshift_ != 0.0) {
      px = px + yshift_ * cos(pth + M_PI_2);
      py = py + yshift_ * sin(pth + M_PI_2);
    }

goal_front_costs_、alignment_costs_时，xshift_不是0。

图2显示了当xshift_不是0时，新(px, py)的计算过程。

    //we won't allow trajectories that go off the map... shouldn't happen that often anyways
    if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {
      //we're off the map
      ROS_WARN("Off Map %f, %f", px, py);
      return -4.0;
    }
    grid_dist = getCellCosts(cell_x, cell_y);
    //if a point on this trajectory has no clear path to the goal... it may be invalid
    if (stop_on_failure_) {
      if (grid_dist == map_.obstacleCosts()) {
        return -3.0;
      } else if (grid_dist == map_.unreachableCellCosts()) {
        return -2.0;
      }
    }

在MapGridCostFunction::scoreTrajectory函数中，依次获取路径中的各个点坐标，并尝试将坐标转为地图坐标，如果转化失败，则scoreTrajecotry函数直接返回-4。否则，从MapGrid地图中找到距离局部地图路径的距离，并将该距离（栅格级）值作为该点的得分。

另外，对于path_costs_和goal_costs_两个打分项的stop_on_failure参数为true，即如果轨迹中的某个点是障碍物或不可到达，则表示查找该点的得分失败，这会造成scoreTrajectory函数直接返回对应的负的得分。例如，如果轨迹中的某个点本身就是障碍物（在路径MapGrid中会有标记），则scoreTrajectory直接返回-3作为轨迹得分；如果轨迹中某个点不可达（在MapGrid地图中没有计算到），则scoreTrajectory直接返回-2作为轨迹得分。

对于goal_front_costs和alignment_costs_两个打分项对应的stop_on_failure参数为false，这是因为前向打分点有可能会出现计算距离出错的情况，例如超出地图范围，但由于前向打分点的打分失败不会带来危险，因此可以不立刻返回负得分。

    switch( aggregationType_ ) {
    case Last:
      cost = grid_dist;
      break;
    case Sum:
      cost += grid_dist;
      break;
    case Product:
      if (cost > 0) {
        cost *= grid_dist;
      }
      break;
    }

轨迹所有点得分的汇总方式有Last、Sum、Product三种方式，默认为Last（且没有对应的配置参数，除非自己修改源码实现），即只考虑轨迹最后一个点的打分。

  }
  return cost;
}

三、MapGrid

图3是示例，用户在距离机器人中心(1米, -2米)处设置导航goal，局部代价地图尺寸是(3米 x 3米)。为直观理解，以下会用这个示例。

3.1 MapGrid::setLocalGoal

void MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap,
    const std::vector<geometry_msgs::PoseStamped>& global_plan) {
  sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());

map_当前尺寸和参数costmap要求的尺寸不一致时，sizeCheck会销毁旧的map_、生成新的。发生重建时，会初始化各MapCell的cx、cy，但不会把target_dist设为unreachableCellCosts，不知算不算不bug。但还没遇到sizeCheck需要重建情况，是bug也不会出错了。

  int local_goal_x = -1;
  int local_goal_y = -1;
  bool started_path = false;

  std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
  adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());

adjustPlanResolution对映射到局部地图中的全局路径进行栅格级插补，从而使得整个路径是栅格连续的。插补思路是这样的，1）所有全局路径的坐标点都会保留。2）相邻两个坐标的距离（直角三角形斜边）超过resolution时，内插坐标点，确保相邻两个坐标距离<=resolution。

  // skip global path points until we reach the border of the local map
  for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i) {
    double g_x = adjusted_global_plan[i].pose.position.x;
    double g_y = adjusted_global_plan[i].pose.position.y;
    unsigned int map_x, map_y;
    if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
      local_goal_x = map_x;
      local_goal_y = map_y;
      started_path = true;
    } else {
      if (started_path) {
        break;
      }// else we might have a non pruned path, so we just continue
    }
  }

for循环目的：找到全局路径中第一个离开局部地图的坐标点。

通过图4中的adjusted_global_plan，可看到存储的路径点中，front是靠近机器中心的坐标，back是靠近目标的坐标。由于局部地图垂直半径是1.5米，adjusted_global_plan在快到1.5米的坐标停止了。for找出距离目标最近、但还落在局部地图的坐标是adjusted_global_plan[62]。至此，local_goal_x、local_goal_y存储着[62]的map类型坐标，由于该坐标所在的行在局部地图最南端，又是个map坐标系坐标，local_goal_y就指向局部地图第一行，即local_goal_y = 0。

  if (!started_path) {
    ROS_ERROR("None of the points of the global plan were in the local costmap, global plan points too far from robot");
    return;
  }

  queue<MapCell*> path_dist_queue;
  if (local_goal_x >= 0 && local_goal_y >= 0) {
    MapCell& current = getCell(local_goal_x, local_goal_y);
    costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
    current.target_dist = 0.0;
    current.target_mark = true;
    path_dist_queue.push(&current);

接下要计算局部地图中所有点到[62]坐标的距离。对[62]，把target_mark置为true，表示不必再以它为中心去膨胀了。 

  }
  computeTargetDistance(path_dist_queue, costmap);
}

3.2 MapGrid::computeTargetDistance

参数dist_queue称为零线，computeTargetDistance采用类似dijkstra算法的逐步探索的方式，计算出map_(MapGrid地图)中所有栅格与零线的最短距离。零线中可能是一个点，也可能是多个点，这些点的target_dist(距离)已置为0、target_mark置为true。

void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
  ...
  while(!dist_queue.empty()){
    current_cell = dist_queue.front();

    dist_queue.pop();
    ...
  }
}

算法结束后会改修改各个MapCell的target_dist。以零点(43, 0)为例，看周围几个点的距离情况。

在图5，左侧是y=0时，x=43周围点的值。相邻点(42, 0)、(44, 0)距离是1，隔一栅格距离增1。中间是y=1时，x=43周围点的值。正向(43, 1)距离1，斜向(42, 1)、(44, 1)距离是2。2是如何计算的？——以(42, 1)为例，由零点(43, 0)膨胀出(43, 1)，这是第一次膨胀，距离加1变成1。由(43, 1)膨胀出(42, 1)，这是第二次膨胀，距离加1变成2。注意，区分这里的距离和直角三角形斜边，按斜边算的话，(42,1)的距离应该是“根号2”。

图5右侧，有些点的距离出现了3600、3601，要理解这个让看下updatePathCell。

inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell,
    const costmap_2d::Costmap2D& costmap){

  //if the cell is an obstacle set the max path distance
  unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
  if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
      (cost == costmap_2d::LETHAL_OBSTACLE ||
       cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
       cost == costmap_2d::NO_INFORMATION)){
    check_cell->target_dist = obstacleCosts();
obstacleCosts()数值是map_.size()，示例就是3600。
    return false;
  }

  double new_target_dist = current_cell->target_dist + 1;
  if (new_target_dist < check_cell->target_dist) {
    check_cell->target_dist = new_target_dist;
  }
  return true;
}

有个疑问，computeTargetDistance会膨胀出代价地图中的所有点吗？——答案会，但前提是代价地图中不能存在距离是obstacleCosts(3600)的点。updatePathCell遇到这些点，会强制以它终止膨胀。那些无法膨胀到的点就保留初始化后的默认值，即unreachableCellCosts(3601)。

除特殊值3600、3601，让估计下距离的最大值。综上所述，要计算点A到零线点的距离，就是从零线点开始，依次画两条直线。第一条垂直，到达A所在的y行，假设那个点是b；第二条水平，由b到A，这两条线经过的栅格数就是距离。于是理论上最大距离就是“size_x_ + size_y_”，对60x60代价地图，最大值是120。所在点直观看就是对角线上的两个点，实际中(local_goal_x, local_goal_y)不大可能恰好是(0, 0)或(59, 0)，也就不大可能到最大值，示例中最大距离是102，其栅格坐标(0, 59)。

3.3 MapGrid::setTargetCells

setTargetCells是用来计算轨迹MapGrid地图中所有点到全局路径的最短距离。与setLocalGoal函数的区别主要在于，它的零线不是一个点，而是全局路径中所有落在局部地图内那些点。

void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,
    const std::vector<geometry_msgs::PoseStamped>& global_plan) {
  sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY());

  bool started_path = false;

  queue<MapCell*> path_dist_queue;

  std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
  adjustPlanResolution(global_plan, adjusted_global_plan, costmap.getResolution());
  if (adjusted_global_plan.size() != global_plan.size()) {
    ROS_DEBUG("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
  }
  unsigned int i;
  // put global path points into local map until we reach the border of the local map
  for (i = 0; i < adjusted_global_plan.size(); ++i) {
    double g_x = adjusted_global_plan[i].pose.position.x;
    double g_y = adjusted_global_plan[i].pose.position.y;
    unsigned int map_x, map_y;
    if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
      MapCell& current = getCell(map_x, map_y);
      current.target_dist = 0.0;
      current.target_mark = true;
      path_dist_queue.push(&current);
      started_path = true;
    } else if (started_path) {
        break;
    }
  }

for循环目的：把全局路径的坐标逐个存入零线（path_dist_queue），直到遇到第一个离开局部地图的坐标点。

在这里，adjusted_global_plan全部坐标都在局部地图，于是图6中adjusted_global_plan和path_dist_queue的size相等。右侧图中可看到，所有零线中的点，距离都置了0。

  if (!started_path) {
    ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
        i, adjusted_global_plan.size(), global_plan.size());
    return;
  }
  computeTargetDistance(path_dist_queue, costmap);
}

computeTargetDistance结束后会改修改各个MapCell的target_dist。以零点(42, 2)为例，看它周围几个点的距离情况。

在图7，左侧是y=2时，x=42周围点的值。相邻点(41, 2)、(43, 2)距离是1，隔一栅格距离增1。右侧是y=3时，x=42周围点的值。正向(42, 3)距离1，斜向(41, 3)、(43, 3)距离是0、2。按之前对setLocalGoal理解，这两个坐标都应该是2，但为什么(41, 3)变成了0？——这正是Local和Target不同的地方，因为(41, 3)也在零线，零线内的都是0。

让继续理解(40, 3)。假设是零点(42, 2)先膨胀出(40, 3)，距离应该是3。然后零点(41, 3)膨胀出(40, 3)，这里距离是1，由于小于3，会覆盖掉(40, 3)之前的3。在setTargetCells，地图中点到全局路径的距离，是该点到全局路径上任何一个点的最短距离。

和setLocalGoal一样原因，距离也会出现3600、3601。

至于可能的最大距离，由于零线是一条线，含有多个点，最大距离通常会比setLocalGoal时小。示例中最大距离是68，其栅格坐标(18, 59)。