ROS : Navigation 解决 “The goal sent to the global planner is off the global costmap...” 的警告

本文档主要记录了 ROS 运行建图和导航节点时，出现 “The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.” 的警告，最终导致导航失败的问题。

问题描述

ROS 启动建图和导航节点，一开始生成的 map 的 size 可能会随着建图过程而扩增，但是代价地图在初始化时已经确定了静态地图层的大小，并不会随着 map 的 size 的变大而变大。此时，导航到 map 后来扩增的区域会报如下警告，最终导致全局规划失败：

[ WARN] [1591176383.391142366, 2794.963000000]: The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.
[ERROR] [1591176383.749939163, 2795.290000000]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors
[WARN] [1591176383.783059, 2795.324000]: Failed to find a valid plan. Even after executing recovery behaviors.

解决思路

研究了一轮 move_base 和 costmap_2d 功能包的源码，最终发现导致上述问题的主要原因是 静态地图层没有随 /map 地图 resize ，而且在最新的源码（ Commits on Mar 10, 2020 ）中已经解决了这个 bug ，解决方案见： fix usage of size_locked, fixes #959 (#966) (#972)

源码修改

修改 static_layer.cpp 中的函数 StaticLayer::incomingMap ：

// resize costmap if size, resolution or origin do not match
Costmap2D* master = layered_costmap_->getCostmap();
if (!layered_costmap_->isRolling() &&
    // !layered_costmap_->isSizeLocked() &&  // mod
    (master->getSizeInCellsX() != size_x ||
     master->getSizeInCellsY() != size_y ||
     master->getResolution() != new_map->info.resolution ||
     master->getOriginX() != new_map->info.origin.position.x ||
     master->getOriginY() != new_map->info.origin.position.y))
{
  // Update the size of the layered costmap (and all layers, including this one)
  ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
  layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
                              new_map->info.origin.position.y,
                              true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);  // mod
}
else if (size_x_ != size_x || size_y_ != size_y ||
         resolution_ != new_map->info.resolution ||
         origin_x_ != new_map->info.origin.position.x ||
         origin_y_ != new_map->info.origin.position.y)
{
  // only update the size of the costmap stored locally in this layer
  ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
  resizeMap(size_x, size_y, new_map->info.resolution,
            new_map->info.origin.position.x, new_map->info.origin.position.y);
}

源码编译：

root@eln-pc:~/catkin_ws# catkin_make -DCATKIN_WHITELIST_PACKAGES="costmap_2d"

这样，代价地图便能随着 map 的 size 的改变而改变。