本篇記錄局部規(guī)劃器生成路徑時(shí)用到的幾個(gè)“輔助工具”富腊，內(nèi)容簡(jiǎn)單也比較少逆屡。MapCell類(lèi)與MapGrid類(lèi)用于獲取軌跡點(diǎn)與目標(biāo)點(diǎn)/全局路徑之間的距離高诺，為路徑打分提供參考，CostmapModel類(lèi)則能夠獲取點(diǎn)峭梳、連線舰绘、多邊形邊緣（機(jī)器人足跡）的cost，是局部規(guī)劃器與costmap間的一個(gè)橋梁葱椭。

【結(jié)構(gòu)示意圖】

【相關(guān)文件】

• 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
  本篇記錄對(duì)MapCell捂寿、MapGrid類(lèi)和CostmapModel類(lèi)閱讀和理解。

【代碼分析】

一孵运、 MapCell類(lèi)與MapGrid類(lèi)

這兩個(gè)類(lèi)是局部規(guī)劃器專(zhuān)門(mén)用來(lái)確定各cell與目標(biāo)點(diǎn)和全局規(guī)劃路徑的距離的秦陋，它們是TrajectoryPlanner的成員，在為路徑打分時(shí)被使用治笨。

• MapGrid類(lèi)數(shù)據(jù)成員：

 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

MapGrid是“地圖”驳概，它包含一個(gè)由MapCell類(lèi)對(duì)象數(shù)組的成員，即cell數(shù)組旷赖，地圖大小為size_x_×size_y_顺又。goal_map和path_map都是MapGrid類(lèi)實(shí)例，這里就分別稱(chēng)這兩張地圖為 “目標(biāo)地圖” 和 “路徑地圖”杠愧。

• MapCell類(lèi)數(shù)據(jù)成員：

 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

MapCell就代表地圖上的一個(gè)cell待榔，它記錄x、y坐標(biāo)（索引）流济。target_dist表示目標(biāo)距離锐锣，它被初始化為無(wú)窮大，經(jīng)過(guò)path_map和goal_map的計(jì)算后绳瘟，它可以表示該cell距目標(biāo)點(diǎn)/全局路徑的距離雕憔。target_mark是該點(diǎn)已更新過(guò)的flag。within_robot表示該點(diǎn)在機(jī)器人足跡范圍內(nèi)糖声。

二斤彼、MapGrid::setTargetCells 處理路徑地圖

首先調(diào)用類(lèi)內(nèi)sizeCheck函數(shù)分瘦，檢查路徑地圖尺寸是否和costmap相同，若不同琉苇，則按照costmap尺寸對(duì)其重新設(shè)置嘲玫，并且檢查MapCell在MapGrid中的index是否和它本身的坐標(biāo)索引一致。

  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; //用于儲(chǔ)存全局路徑上的MapCell

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

傳入的全局規(guī)劃是global系下的并扇，調(diào)用adjustPlanResolution函數(shù)對(duì)其分辨率進(jìn)行一個(gè)簡(jiǎn)單的調(diào)整去团，使其達(dá)到costmap的分辨率，方法也很簡(jiǎn)單穷蛹，當(dāng)相鄰點(diǎn)之間距離>分辨率土陪，在其間插入點(diǎn)，以達(dá)到分辨率要求肴熏。

    // 調(diào)整全局路徑的分辨率使其能夠達(dá)到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;

接下來(lái)將全局路徑的點(diǎn)轉(zhuǎn)換到“路徑地圖”上鬼雀，并將對(duì)應(yīng)cell的target_dist設(shè)置為0，標(biāo)記已計(jì)算過(guò)蛙吏。由于分辨率已經(jīng)過(guò)調(diào)整源哩，故不會(huì)出現(xiàn)路徑不連續(xù)的情況。在“路徑地圖”上得到一條在地圖范圍內(nèi)出刷、且不經(jīng)過(guò)costmap上未知cell的全局路徑璧疗，每個(gè)點(diǎn)的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;
      // 如果成功把一個(gè)全局規(guī)劃上的點(diǎn)的坐標(biāo)轉(zhuǎn)換到地圖坐標(biāo)（map_x, map_y）上馁龟，且在代價(jià)地圖上這一點(diǎn)不是未知的
      if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
        // 用current來(lái)引用這個(gè)cell
        MapCell& current = getCell(map_x, map_y);
        // 將這個(gè)點(diǎn)的target_dist設(shè)置為0崩侠，即在全局路徑上
        current.target_dist = 0.0;
        // 標(biāo)記已經(jīng)計(jì)算了距離
        current.target_mark = true;
        // 把該點(diǎn)放進(jìn)path_dist_queue隊(duì)列中
        path_dist_queue.push(&current);
        // 標(biāo)記已經(jīng)開(kāi)始把點(diǎn)轉(zhuǎn)換到地圖坐標(biāo)
        started_path = true;
      } 
      // 當(dāng)代價(jià)地圖上這一點(diǎn)的代價(jià)不存在了（規(guī)劃路徑已經(jīng)到達(dá)了代價(jià)地圖的邊界）
      // 并且標(biāo)記了已經(jīng)開(kāi)始轉(zhuǎn)換，退出循環(huán)
      else if (started_path) {
          break;
      }
    }
    // 如果循環(huán)結(jié)束后并沒(méi)有確定任何在地圖坐標(biāo)上的點(diǎn)
    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;
    }

接下來(lái)調(diào)用類(lèi)內(nèi)computeTargetDistance函數(shù)坷檩，開(kāi)始“填充”却音，最終得到一張完整的路徑地圖。

   // 計(jì)算本地地圖上的每一個(gè)cell與規(guī)劃路徑之間的距離
   computeTargetDistance(path_dist_queue, costmap);
 }

三矢炼、MapGrid::setLocalGoal 處理目標(biāo)地圖

同樣系瓢，先調(diào)整全局規(guī)劃的分辨率。

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

然后句灌，通過(guò)迭代找到全局路徑的終點(diǎn)夷陋，即目標(biāo)點(diǎn)，但如果迭代過(guò)程當(dāng)中到達(dá)了局部規(guī)劃costmap的邊際或經(jīng)過(guò)障礙物胰锌，立即退出迭代骗绕，將上一個(gè)有效點(diǎn)作為終點(diǎn)。

    // 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酬土，嘗試找到終點(diǎn)
        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;
    }

將迭代得到的目標(biāo)點(diǎn)對(duì)應(yīng)在“目標(biāo)地圖”上的cell的target_dist標(biāo)記為0。setTargetCells和setLocalGoal這兩個(gè)函數(shù)格带，前者在“路徑地圖”上將全局路徑點(diǎn)target_dist標(biāo)記為0撤缴，后者在“目標(biāo)地圖”上將目標(biāo)點(diǎn)target_dist標(biāo)記為0刹枉。最后同樣調(diào)用computeTargetDistance函數(shù)。

    queue<MapCell*> path_dist_queue;
    // 如果找到的目標(biāo)點(diǎn)橫縱坐標(biāo)都大于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);
  }

依次檢查“父cell”四周的cell并標(biāo)記它已計(jì)算屈呕，對(duì)它調(diào)用updatePathCell函數(shù)微宝，得到該cell的值。若有效凉袱，加入循環(huán)隊(duì)列里芥吟，彈出“父cell”侦铜，四周的cell成為新的“父cell”专甩， 循環(huán)，直到所有cell更新完畢钉稍。

check_cell其實(shí)是children_cell涤躲，是current_cell的子節(jié)點(diǎn)，computeTargetDistance其實(shí)是個(gè)dijstra算法

      // 如果該點(diǎn)的橫坐標(biāo)大于0
      if(current_cell->cx > 0){
        // 檢查它左側(cè)相鄰的點(diǎn)check_cell
        check_cell = current_cell - 1;
        // 如果check_cell未被標(biāo)記
        if(!check_cell->target_mark){
          // 標(biāo)記它
          check_cell->target_mark = true;
          // updatePathCell功能：如果check_cell不是障礙物贡未，將target_dist設(shè)置為current_cell.target_distance+1种樱；否則，設(shè)置為無(wú)窮
          if(updatePathCell(current_cell, check_cell, costmap)) {
            // check_cell入隊(duì)
            dist_queue.push(check_cell);
          }
        }
      }
      // 如果該點(diǎn)的橫坐標(biāo)小于圖像寬度-1
      if(current_cell->cx < last_col){
        // 檢查它右側(cè)相鄰的點(diǎn)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);
          }
        }
      }
      // 如果該點(diǎn)的縱坐標(biāo)>0
      if(current_cell->cy > 0){
        // 檢查它上面的點(diǎn)
        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);
          }
        }
      }
      // 如果該點(diǎn)的縱坐標(biāo)<圖像長(zhǎng)度-1
      if(current_cell->cy < last_row){
        // 檢查它下面的點(diǎn)
        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);
          }
        }
      }
      // 所以已經(jīng)計(jì)算過(guò)的點(diǎn)不斷被加入俊卤，然后利用它找到周?chē)c(diǎn)后嫩挤，彈出它，直到地圖上所有點(diǎn)的距離都被計(jì)算出來(lái)
    }
  }
};

關(guān)于updatePathCell函數(shù)消恍，先獲取該cell在costmap上的值岂昭，若發(fā)現(xiàn)它是障礙物或未知cell或它在機(jī)器人足跡內(nèi)，直接返回false狠怨，該cell將不會(huì)進(jìn)入循環(huán)隊(duì)列约啊，也就是不會(huì)由它繼續(xù)向下傳播；若非以上情況佣赖，讓它的值=鄰點(diǎn)值+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（本來(lái)通過(guò)resetPathDist被設(shè)置為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;
 }

這和全局規(guī)劃器使用Dijkstra算法時(shí)用costarr生成potarr很類(lèi)似，過(guò)程中的“累加”能夠反映各cell與初始cell之間的距離俩檬，這樣萎胰，最終得到完整的path_map和goal_map。

四豆胸、CostmapModel類(lèi)

這個(gè)類(lèi)幫助局部規(guī)劃器在Costmap上進(jìn)行計(jì)算奥洼，footprintCost、lineCost晚胡、pointCost三個(gè)類(lèi)方法分別能通過(guò)Costmap計(jì)算出機(jī)器人足跡范圍灵奖、兩個(gè)cell連線嚼沿、單個(gè)cell的代價(jià)，并將值返回給局部規(guī)劃器瓷患。這里簡(jiǎn)單看一下footprintCost函數(shù)骡尽。

• 若預(yù)設(shè)的足跡點(diǎn)數(shù)<3，考慮足跡的形狀沒(méi)有意義擅编，這時(shí)只計(jì)算機(jī)器人位置點(diǎn)在costmap上的代價(jià)攀细；

• 若預(yù)設(shè)的足跡點(diǎn)數(shù)≥3，把足跡視為多邊形爱态，循環(huán)調(diào)用lineCost計(jì)算多邊形各邊的cell谭贪，注意首尾閉合，* 最后返回代價(jià)锦担。
  返回值:

• -1.0 ：覆蓋至少一個(gè)障礙cell

• -2.0 ：覆蓋至少一個(gè)未知cell

• -3.0 ：不在地圖上

• 其他正cost

為什么這里只要多邊形邊緣不經(jīng)過(guò)障礙就認(rèn)為足跡不經(jīng)過(guò)障礙呢俭识？而不是多邊形內(nèi)部所有的點(diǎn)都要檢測(cè)呢?

• 1.因?yàn)檎J(rèn)為機(jī)器人是實(shí)心的，一定是最外面的邊緣線碰到障礙物洞渔，所以是檢測(cè)多邊形的邊緣

• 2. 因?yàn)檫@個(gè)函數(shù)是在軌跡點(diǎn)生成過(guò)程中套媚，對(duì)新生成的軌跡點(diǎn)調(diào)用的，而既然能新生成軌跡點(diǎn)磁椒，說(shuō)明該軌跡點(diǎn)必不是障礙物堤瘤，當(dāng)多邊形邊緣不經(jīng)過(guò)障礙、中心也不是障礙的時(shí)候浆熔，如果機(jī)器人比較小本辐，可以認(rèn)為足跡不經(jīng)過(guò)障礙。

  double CostmapModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
      double inscribed_radius, double circumscribed_radius){
    //聲明地圖坐標(biāo)系上的坐標(biāo)
    unsigned int cell_x, cell_y;

    /****************************************考慮機(jī)器人中心******************************************/
    //得到機(jī)器人中心點(diǎn)的cell坐標(biāo)蘸拔，存放在cell_x cell_y中
    //如果得不到坐標(biāo)师郑，說(shuō)明不在地圖上，直接返回-3
    if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
      return -3.0;

    //如果腳拥髑稀（預(yù)先輸入的）點(diǎn)數(shù)小于三宝冕，默認(rèn)機(jī)器人形狀為圓形，不考慮腳印邓萨，只考慮中心
    if(footprint.size() < 3){
      unsigned char cost = costmap_.getCost(cell_x, cell_y); 
      //如果中心位于未知代價(jià)的cell上地梨，返回-2
      if(cost == NO_INFORMATION)
        return -2.0;
      //如果中心位于致命障礙cell上，返回-1
      if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE) //這幾個(gè)宏是在costvalue中定義的缔恳，是灰度值
        return -1.0;
      //如果機(jī)器人位置既不是未知也不是致命宝剖，返回它的代價(jià)
      return cost;
    }

    /****************************************考慮機(jī)器人腳印******************************************/
    //接下來(lái)在costmap上考慮腳印
    unsigned int x0, x1, y0, y1;
    double line_cost = 0.0; //初始化
    double footprint_cost = 0.0; //初始化0

    //對(duì)footprint進(jìn)行光柵化算法處理得到柵格點(diǎn)
    //計(jì)算各個(gè)腳印點(diǎn)連成的多邊形的邊緣上是否碰撞到障礙物
    //i是索引，大小是腳印的size歉甚，第一次万细，i=1，下面得到footprint[1]和footprint[2]的連線的cost纸泄，后面以此類(lèi)推
    for(unsigned int i = 0; i < footprint.size() - 1; ++i){
      //得到腳印中第一個(gè)點(diǎn)的坐標(biāo)x0, y0
      if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
        //如果無(wú)法轉(zhuǎn)換赖钞，說(shuō)明第一個(gè)腳印點(diǎn)不在地圖上腰素，直接返回-3
        return -3.0;

      //得到腳印中下一個(gè)點(diǎn)的坐標(biāo)x1, y1
      if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
        //如果無(wú)法轉(zhuǎn)換，說(shuō)明下一個(gè)腳印點(diǎn)不在地圖上雪营，直接返回-3
        return -3.0;

      //得到連線的代價(jià)
      line_cost = lineCost(x0, x1, y0, y1);
      //不斷用最大的連線代價(jià)迭代footprint_cost
      footprint_cost = std::max(line_cost, footprint_cost);

      //如果某條邊緣線段代價(jià)<0弓千，直接停止生成代價(jià)，返回這個(gè)負(fù)代價(jià)
      if(line_cost < 0) 
        return line_cost;
    }
    //再把footprint的最后一個(gè)點(diǎn)和第一個(gè)點(diǎn)連起來(lái)献起，形成閉合
    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;

    //如果所有邊緣線的代價(jià)都是合法的洋访，那么返回足跡的代價(jià)
    return footprint_cost;
  }

轉(zhuǎn)載：https://blog.csdn.net/Neo11111/article/details/104720103#commentBox