Robotics StackExchange | Archived questions

map update loop missed its desired rate

HI,

So my problem is that I have a relatively big map 2144 by 2528 pixels and this leads to big latency in the global costmap update it usually misses the desired rate of 1.0 HZ. I realize that reducing the resolution might be a solution but I do not want to reduce the accuracy of the map also. What I am thinking about is modifying the map server to ignore the gray area of the map because usually it takes big space in the map image and I do not want to parse it, Therefore later we would have less map cells to update.

I want to ask your opinions about this solution, and your own experiences with such problems.

Thank you.

Asked by starter on 2018-09-11 05:32:46 UTC

Comments

Answers

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

Asked by Namal Senarathne on 2018-09-13 20:50:49 UTC

Comments

Thank you for reply, actually after some profiling I found out that the inflation layer is the source of the problem it takes a very long time to update. All my layers combined take nearly 200 ms to update and the inflation layer takes almost 1.5 s. Do you have a solution for that ?

Asked by starter on 2018-09-18 02:49:09 UTC

Well I assumed inflation works only with changes to the incoming map data however I haven't closely looked at the implementation. It may be that it is computing the inflation for the map every iteration, which could explain the 1.5s update time.

Asked by Namal Senarathne on 2018-09-20 20:11:38 UTC

One possible solution is to partition your big map and dynamically load the portion that surrounds the robot during the current step, but this requires changes to some of the navigation as well similar to http://wiki.ros.org/topological_navigation (this is no longer maintained it seems)

Asked by Namal Senarathne on 2018-09-20 20:14:30 UTC