【ROS-Navigation】Base Local Planner局部规划-TrajectoryPlanner源码解读-3

记录学习阅读ROS Navigation源码的理解，本文为Base Local Planner局部规划源码学习记录，以文字总结、绘制结构图说明、代码注释为主。仍在学习过程中，有错误欢迎指正，共同进步。

本篇记录局部规划器生成路径时用到的几个“辅助工具”，内容简单也比较少。MapCell类与MapGrid类用于获取轨迹点与目标点/全局路径之间的距离，为路径打分提供参考，CostmapModel类则能够获取点、连线、多边形边缘（机器人足迹）的cost，是局部规划器与costmap间的一个桥梁。

【结构示意图】

【相关文件】

• base_local_planner/src/trajectory_planner_ros.cpp
• base_local_planner/src/trajectory_planner.cpp
• base_local_planner/src/map_grid.cpp
• base_local_planner/src/map_cell.cpp
• base_local_planner/src/costmap_model.cpp

本篇记录对MapCell、MapGrid类和CostmapModel类阅读和理解。

【代码分析】

1. MapCell类与MapGrid类

这两个类是局部规划器专门用来确定各cell与目标点和全局规划路径的距离的，它们是TrajectoryPlanner的成员，在为路径打分时被使用。

MapGrid是“地图”，它包含一个由MapCell类对象数组的成员，即cell数组，地图大小为size_x_×size_y_。goal_map和path_map都是MapGrid类实例，这里就分别称这两张地图为 “目标地图” 和 “路径地图”。

MapGrid类数据成员：

 public:
    double goal_x_, goal_y_; /**< @brief The goal distance was last computed from */
    unsigned int size_x_, size_y_; ///< @brief The dimensions of the grid
 private:
    std::vector<MapCell> map_; ///< @brief Storage for the MapCells

MapCell就代表地图上的一个cell，它记录x、y坐标（索引）。target_dist表示目标距离，它被初始化为无穷大，经过path_map和goal_map的计算后，它可以表示该cell距目标点/全局路径的距离。target_mark是该点已更新过的flag。within_robot表示该点在机器人足迹范围内。

MapCell类数据成员：

 public:
     unsigned int cx, cy; ///< @brief Cell index in the grid map
     double target_dist; ///< @brief Distance to planner's path
     bool target_mark; ///< @brief Marks for computing path/goal distances
     bool within_robot; ///< @brief Mark for cells within the robot footprint

下面关注MapGrid的类方法，理解如何更新目标地图和路径地图。

<1> MapGrid::setTargetCells 处理路径地图

首先调用类内sizeCheck函数，检查路径地图尺寸是否和costmap相同，若不同，则按照costmap尺寸对其重新设置，并且检查MapCell在MapGrid中的index是否和它本身的坐标索引一致。

  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; //用于储存全局路径上的MapCell

    std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;

传入的全局规划是global系下的，调用adjustPlanResolution函数对其分辨率进行一个简单的调整，使其达到costmap的分辨率，方法也很简单，当相邻点之间距离>分辨率，在其间插入点，以达到分辨率要求。

    // 调整全局路径的分辨率使其能够达到cost_map的分辨率
    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;

接下来将全局路径的点转换到“路径地图”上，并将对应cell的target_dist设置为0，标记已计算过。由于分辨率已经过调整，故不会出现路径不连续的情况。在“路径地图”上得到一条在地图范围内、且不经过costmap上未知cell的全局路径，每个点的target_dist都为0。

    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;
      // 如果成功把一个全局规划上的点的坐标转换到地图坐标（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) {
        // 用current来引用这个cell
        MapCell& current = getCell(map_x, map_y);
        // 将这个点的target_dist设置为0，即在全局路径上
        current.target_dist = 0.0;
        // 标记已经计算了距离
        current.target_mark = true;
        // 把该点放进path_dist_queue队列中
        path_dist_queue.push(&current);
        // 标记已经开始把点转换到地图坐标
        started_path = true;
      } 
      // 当代价地图上这一点的代价不存在了（规划路径已经到达了代价地图的边界）
      // 并且标记了已经开始转换，退出循环
      else if (started_path) {
          break;
      }
    }
    // 如果循环结束后并没有确定任何在地图坐标上的点
    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函数，开始“填充”，最终得到一张完整的路径地图。

   // 计算本地地图上的每一个cell与规划路径之间的距离
   computeTargetDistance(path_dist_queue, costmap);
 }

<2> MapGrid::setLocalGoal 处理目标地图

同样，先调整全局规划的分辨率。

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

    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());

然后，通过迭代找到全局路径的终点，即目标点，但如果迭代过程当中到达了局部规划costmap的边际或经过障碍物，立即退出迭代，将上一个有效点作为终点。

    // 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、local_goal_y，尝试找到终点
        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
      }
    }
    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;
    }

将迭代得到的目标点对应在“目标地图”上的cell的target_dist标记为0。setTargetCells和setLocalGoal这两个函数，前者在“路径地图”上将全局路径点target_dist标记为0，后者在“目标地图”上将目标点target_dist标记为0。最后同样调用computeTargetDistance函数。

    queue<MapCell*> path_dist_queue;
    // 如果找到的目标点横纵坐标都大于0
    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);
    }
    computeTargetDistance(path_dist_queue, costmap);
  }

<3> MapGrid::computeTargetDistance 填充地图

这个函数传入target_dist=0.0的cell队列，然后从它们开始，向四周开始“传播”。

  void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
    MapCell* current_cell;
    MapCell* check_cell;
    unsigned int last_col = size_x_ - 1;
    unsigned int last_row = size_y_ - 1;
    // 当队列非空，循环
    while(!dist_queue.empty()){
      current_cell = dist_queue.front();
	  //出队
      dist_queue.pop();

依次检查“父cell”四周的cell并标记它已计算，对它调用updatePathCell函数，得到该cell的值。若有效，加入循环队列里，弹出“父cell”，四周的cell成为新的“父cell”， 循环，直到所有cell更新完毕。

      // 如果该点的横坐标大于0
      if(current_cell->cx > 0){
        // 检查它左侧相邻的点check_cell
        check_cell = current_cell - 1;
        // 如果check_cell未被标记
        if(!check_cell->target_mark){
          // 标记它
          check_cell->target_mark = true;
          // updatePathCell功能：如果check_cell不是障碍物，将target_dist设置为current_cell+1；否则，设置为无穷
          if(updatePathCell(current_cell, check_cell, costmap)) {
            // check_cell入队
            dist_queue.push(check_cell);
          }
        }
      }
      // 如果该点的横坐标小于图像宽度-1
      if(current_cell->cx < last_col){
        // 检查它右侧相邻的点check_cell
        check_cell = current_cell + 1;
        if(!check_cell->target_mark){
          check_cell->target_mark = true;
          if(updatePathCell(current_cell, check_cell, costmap)) {
            dist_queue.push(check_cell);
          }
        }
      }
      // 如果该点的纵坐标>0
      if(current_cell->cy > 0){
        // 检查它上面的点
        check_cell = current_cell - size_x_;
        if(!check_cell->target_mark){
          check_cell->target_mark = true;
          if(updatePathCell(current_cell, check_cell, costmap)) {
            dist_queue.push(check_cell);
          }
        }
      }
      // 如果该点的纵坐标<图像长度-1
      if(current_cell->cy < last_row){
        // 检查它下面的点
        check_cell = current_cell + size_x_;
        if(!check_cell->target_mark){
          check_cell->target_mark = true;
          if(updatePathCell(current_cell, check_cell, costmap)) {
            dist_queue.push(check_cell);
          }
        }
      }
      // 所以已经计算过的点不断被加入，然后利用它找到周围点后，弹出它，直到地图上所有点的距离都被计算出来
    }
  }
};

关于updatePathCell函数，先获取该cell在costmap上的值，若发现它是障碍物或未知cell或它在机器人足迹内，直接返回false，该cell将不会进入循环队列，也就是不会由它继续向下传播；若非以上情况，让它的值=邻点值+1，若<原值，用它更新。

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

   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();
     return false;
   }

   // 不是障碍物的话，比较它的target_dist（本来通过resetPathDist被设置为unreachableCellCosts）与>current_cell->target_dist + 1
   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;
 }

这和全局规划器使用Dijkstra算法时用costarr生成potarr很类似，过程中的“累加”能够反映各cell与初始cell之间的距离，这样，最终得到完整的path_map和goal_map。

2. CostmapModel类

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

• 若预设的足迹点数<3，考虑足迹的形状没有意义，这时只计算机器人位置点在costmap上的代价；
• 若预设的足迹点数≥3，把足迹视为多边形，循环调用lineCost计算多边形各边的cell，注意首尾闭合，最后返回代价。

返回值:

• -1.0 ：覆盖至少一个障碍cell
• -2.0 ：覆盖至少一个未知cell
• -3.0 ：不在地图上
• 其他正cost

为什么这里只要多边形边缘不经过障碍就认为足迹不经过障碍呢？ 因为这个函数是在轨迹点生成过程中，对新生成的轨迹点调用的，而既然能新生成轨迹点，说明该轨迹点必不是障碍物，当多边形边缘不经过障碍、中心也不是障碍的时候，如果机器人比较小，可以认为足迹不经过障碍。

  double CostmapModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
      double inscribed_radius, double circumscribed_radius){
    //声明地图坐标系上的坐标
    unsigned int cell_x, cell_y;

    /****************************************考虑机器人中心******************************************/
    //得到机器人中心点的cell坐标，存放在cell_x cell_y中
    //如果得不到坐标，说明不在地图上，直接返回-3
    if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
      return -3.0;

    //如果脚印（预先输入的）点数小于三，默认机器人形状为圆形，不考虑脚印，只考虑中心
    if(footprint.size() < 3){
      unsigned char cost = costmap_.getCost(cell_x, cell_y); 
      //如果中心位于未知代价的cell上，返回-2
      if(cost == NO_INFORMATION)
        return -2.0;
      //如果中心位于致命障碍cell上，返回-1
      if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE) //这几个宏是在costvalue中定义的，是灰度值
        return -1.0;
      //如果机器人位置既不是未知也不是致命，返回它的代价
      return cost;
    }

    /****************************************考虑机器人脚印******************************************/
    //接下来在costmap上考虑脚印
    unsigned int x0, x1, y0, y1;
    double line_cost = 0.0; //初始化
    double footprint_cost = 0.0; //初始化0

    //对footprint进行光栅化算法处理得到栅格点
    //计算各个脚印点连成的多边形的边缘上是否碰撞到障碍物
    //i是索引，大小是脚印的size，第一次，i=1，下面得到footprint[1]和footprint[2]的连线的cost，后面以此类推
    for(unsigned int i = 0; i < footprint.size() - 1; ++i){
      //得到脚印中第一个点的坐标x0, y0
      if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
        //如果无法转换，说明第一个脚印点不在地图上，直接返回-3
        return -3.0;

      //得到脚印中下一个点的坐标x1, y1
      if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
        //如果无法转换，说明下一个脚印点不在地图上，直接返回-3
        return -3.0;

      //得到连线的代价
      line_cost = lineCost(x0, x1, y0, y1);
      //不断用最大的连线代价迭代footprint_cost
      footprint_cost = std::max(line_cost, footprint_cost);

      //如果某条边缘线段代价<0，直接停止生成代价，返回这个负代价
      if(line_cost < 0) 
        return line_cost;
    }
    //再把footprint的最后一个点和第一个点连起来，形成闭合
    if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
      return -3.0;
    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;

    //如果所有边缘线的代价都是合法的，那么返回足迹的代价
    return footprint_cost;
  }

Neo 2020.3