Method MapUpdateLoop

roborts_costmap/src/costmap_interface.cpp:206–257

}

void CostmapInterface::MapUpdateLoop(double frequency) {
  if (frequency <= 0.0) {
    ROS_ERROR("Frequency must be positive in MapUpdateLoop.");
  }
  ros::NodeHandle nh;
  ros::Rate r(frequency);
  while (nh.ok() && !map_update_thread_shutdown_) {
    struct timeval start, end;
    gettimeofday(&start, NULL);
    UpdateMap();
    gettimeofday(&end, NULL);
    r.sleep();
    if (r.cycleTime() > ros::Duration(1 / frequency)) {
      if (is_debug_) {
        ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds",\
        frequency, r.cycleTime().toSec());
      }
    }
    double x, y;
    Costmap2D *temp_costmap = layered_costmap_->GetCostMap();
    unsigned char *data = temp_costmap->GetCharMap();
    temp_costmap->Map2World(0, 0, x, y);
    grid_.header.frame_id = global_frame_;
    grid_.header.stamp = ros::Time::now();
    if (is_rolling_window_) {
      grid_.info.resolution = map_resolution_;
      grid_.info.width = map_width_ / map_resolution_;
      grid_.info.height = map_height_ / map_resolution_;
      grid_.info.origin.position.x = x - map_resolution_ * 0.5;
      grid_.info.origin.position.y = y - map_resolution_ * 0.5;
      grid_.info.origin.position.z = 0;
      grid_.info.origin.orientation.w = 1.0;
      grid_.data.resize(grid_.info.width * grid_.info.height);
    } else {
      auto resolution = temp_costmap->GetResolution();
      auto map_width = temp_costmap->GetSizeXCell();
      auto map_height = temp_costmap->GetSizeYCell();
      grid_.info.resolution = resolution;
      grid_.info.width = map_width;
      grid_.info.height = map_height;
      grid_.info.origin.position.x = temp_costmap->GetOriginX();
      grid_.info.origin.position.y = temp_costmap->GetOriginY();
      grid_.info.origin.position.z = 0;
      grid_.info.origin.orientation.w = 1.0;
      grid_.data.resize(map_width * map_height);
    }
    for (size_t i = 0; i < grid_.data.size(); i++) {
      grid_.data[i] = cost_translation_table_[data[i]];
    }
    costmap_pub_.publish(grid_);
  }
}

void CostmapInterface::UpdateMap() {
  if (!stop_updates_) {