Ask Your Question

Revision history [back]

map_server publishes the static global map only once for each subscribed node. So the static_layer plugin used by global_costmap reads and parses this big map only once. So I don't see how modifying this would solve your issue.

Have you set always_send_full_costmap parameter of global_costmap to true? If you have set this to true, then the global_costmap generates the full costmap at each update step. If it is set to false (which is the default), it only publishes the updates to the global costmap, which is quite efficient and you shouldn't be getting this error. (see the code below from the cost-map publisher)

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

  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_)
  {
    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
    // 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();
}