【ROS-Navigation】系列(X) ——Base Local Planner局部规划-TrajectoryPlanner源码解读(3/3)

本篇记录局部规划器生成路径时用到的几个“辅助工具”,内容简单也比较少。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类阅读和理解。

【代码分析】

一、 MapCell类与MapGrid类

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

  • 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

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

  • 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

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

二、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);
 }

三、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);
  }

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

check_cell其实是children_cell,是current_cell的子节点,computeTargetDistance其实是个dijstra算法

      // 如果该点的横坐标大于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.target_distance+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。

四、CostmapModel类

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

  • 若预设的足迹点数<3,考虑足迹的形状没有意义,这时只计算机器人位置点在costmap上的代价;

  • 若预设的足迹点数≥3,把足迹视为多边形,循环调用lineCost计算多边形各边的cell,注意首尾闭合,* 最后返回代价。
    返回值:

  • -1.0 :覆盖至少一个障碍cell

  • -2.0 :覆盖至少一个未知cell

  • -3.0 :不在地图上

  • 其他正cost

为什么这里只要多边形边缘不经过障碍就认为足迹不经过障碍呢?而不是多边形内部所有的点都要检测呢?

  • 1.因为认为机器人是实心的,一定是最外面的边缘线碰到障碍物,所以是检测多边形的边缘

    1. 因为这个函数是在轨迹点生成过程中,对新生成的轨迹点调用的,而既然能新生成轨迹点,说明该轨迹点必不是障碍物,当多边形边缘不经过障碍、中心也不是障碍的时候,如果机器人比较小,可以认为足迹不经过障碍。
  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;
  }

转载:https://blog.csdn.net/Neo11111/article/details/104720103#commentBox

最后编辑于
©著作权归作者所有,转载或内容合作请联系作者
  • 序言:七十年代末,一起剥皮案震惊了整个滨河市,随后出现的几起案子,更是在滨河造成了极大的恐慌,老刑警刘岩,带你破解...
    沈念sama阅读 213,254评论 6 492
  • 序言:滨河连续发生了三起死亡事件,死亡现场离奇诡异,居然都是意外死亡,警方通过查阅死者的电脑和手机,发现死者居然都...
    沈念sama阅读 90,875评论 3 387
  • 文/潘晓璐 我一进店门,熙熙楼的掌柜王于贵愁眉苦脸地迎上来,“玉大人,你说我怎么就摊上这事。” “怎么了?”我有些...
    开封第一讲书人阅读 158,682评论 0 348
  • 文/不坏的土叔 我叫张陵,是天一观的道长。 经常有香客问我,道长,这世上最难降的妖魔是什么? 我笑而不...
    开封第一讲书人阅读 56,896评论 1 285
  • 正文 为了忘掉前任,我火速办了婚礼,结果婚礼上,老公的妹妹穿的比我还像新娘。我一直安慰自己,他们只是感情好,可当我...
    茶点故事阅读 66,015评论 6 385
  • 文/花漫 我一把揭开白布。 她就那样静静地躺着,像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上,一...
    开封第一讲书人阅读 50,152评论 1 291
  • 那天,我揣着相机与录音,去河边找鬼。 笑死,一个胖子当着我的面吹牛,可吹牛的内容都是我干的。 我是一名探鬼主播,决...
    沈念sama阅读 39,208评论 3 412
  • 文/苍兰香墨 我猛地睁开眼,长吁一口气:“原来是场噩梦啊……” “哼!你这毒妇竟也来了?” 一声冷哼从身侧响起,我...
    开封第一讲书人阅读 37,962评论 0 268
  • 序言:老挝万荣一对情侣失踪,失踪者是张志新(化名)和其女友刘颖,没想到半个月后,有当地人在树林里发现了一具尸体,经...
    沈念sama阅读 44,388评论 1 304
  • 正文 独居荒郊野岭守林人离奇死亡,尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 36,700评论 2 327
  • 正文 我和宋清朗相恋三年,在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 38,867评论 1 341
  • 序言:一个原本活蹦乱跳的男人离奇死亡,死状恐怖,灵堂内的尸体忽然破棺而出,到底是诈尸还是另有隐情,我是刑警宁泽,带...
    沈念sama阅读 34,551评论 4 335
  • 正文 年R本政府宣布,位于F岛的核电站,受9级特大地震影响,放射性物质发生泄漏。R本人自食恶果不足惜,却给世界环境...
    茶点故事阅读 40,186评论 3 317
  • 文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹,春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 30,901评论 0 21
  • 文/苍兰香墨 我抬头看了看天上的太阳。三九已至,却和暖如春,着一层夹袄步出监牢的瞬间,已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 32,142评论 1 267
  • 我被黑心中介骗来泰国打工, 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留,地道东北人。 一个月前我还...
    沈念sama阅读 46,689评论 2 362
  • 正文 我出身青楼,却偏偏与公主长得像,于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子,可洞房花烛夜当晚...
    茶点故事阅读 43,757评论 2 351

推荐阅读更多精彩内容