ROS costmap2d 不断发布global_costmap

博主最近在做自主全局规划时需要实时获得全局不断更新的global_costmap。

搜索costmap_2d的roswiki官方文档，发现costmap_2d会发布以下两个话题：

然而，经过实际测试，我们订阅第一个话题：/move_base/global_costmap/costmap 却只能收到一次消息，而第二个话题/move_base/global_costmap/costmap_updates在不断发布，但只发布更新部分，也就是只在地图中的一部分。我们如果要手动更新再拼接，会非常麻烦。

初步阅读costmap_2d的源码后，我发现publish函数在此定义：

costmap_2d_publisher.cpp

void Costmap2DPublisher::publishCostmap()
{
  if (costmap_pub_.getNumSubscribers() == 0)
  {
    // No subscribers, so why do any work?
    return;
  }

  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
  float resolution = costmap_->getResolution();

  if (always_send_full_costmap_ || grid_.info.resolution != resolution ||
      grid_.info.width != costmap_->getSizeInCellsX() ||
      grid_.info.height != costmap_->getSizeInCellsY() ||
      saved_origin_x_ != costmap_->getOriginX() ||
      saved_origin_y_ != costmap_->getOriginY())
  {
    prepareGrid();
    costmap_pub_.publish(grid_);
  }
  else if (x0_ < xn_)
  {
    // Publish Just an Update
    map_msgs::OccupancyGridUpdate update;
    update.header.stamp = ros::Time::now();
    update.header.frame_id = global_frame_;
    update.x = x0_;
    update.y = y0_;
    update.width = xn_ - x0_;
    update.height = yn_ - y0_;
    update.data.resize(update.width * update.height);

    unsigned int i = 0;
    for (unsigned int y = y0_; y < yn_; y++)
    {
      for (unsigned int x = x0_; x < xn_; x++)
      {
        unsigned char cost = costmap_->getCost(x, y);
        update.data[i++] = cost_translation_table_[ cost ];
      }
    }
    costmap_update_pub_.publish(update);
  }

  xn_ = yn_ = 0;
  x0_ = costmap_->getSizeInCellsX();
  y0_ = costmap_->getSizeInCellsY();
}

其中costmap_pub_就对应第一个发布话题，costmap_update_pub_对应第二个发布话题。

从源码中可以发现，只有当always_send_full_costmap_参数为真或者地图的参数（原点、高宽、resolution）发生改变时，才会重新发布。然而always_send_full_costmap_在代码中默认定义为假：

private_nh.param("always_send_full_costmap", always_send_full_costmap, false);

这就可以解释为什么不会一直发布全局costmap。

最简单的修改方式是在if语句中加一个true：

if (true || always_send_full_costmap_ || grid_.info.resolution != resolution ||
      grid_.info.width != costmap_->getSizeInCellsX() ||
      grid_.info.height != costmap_->getSizeInCellsY() ||
      saved_origin_x_ != costmap_->getOriginX() ||
      saved_origin_y_ != costmap_->getOriginY())
  {
    prepareGrid();
    costmap_pub_.publish(grid_);
  }

经过测试，全局costmap也会一直发布。

note：博主发现发布频率跟不上设定频率，有可能是算力受限，可能需要将全局地图缩小。一直发布也可能占用一些算力。

---------------------------------------------------------------------------------------------------------------------------------更新：博主发现可以直接改参数QAQ