Method UpdateMap

roborts_costmap/src/layered_costmap.cpp:94–140

}

void CostmapLayers::UpdateMap(double robot_x, double robot_y, double robot_yaw) {
  static int count = 0;
  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.GetMutex()));
  if (is_rolling_window_) {
    double new_origin_x = robot_x - costmap_.GetSizeXWorld() / 2;
    double new_origin_y = robot_y - costmap_.GetSizeYWorld() / 2;
    costmap_.UpdateOrigin(new_origin_x, new_origin_y);
  }
  if (plugins_.size() == 0) {
    ROS_WARN("No Layer");
    return;
  }

  minx_ = miny_ = 1e30;
  maxx_ = maxy_ = -1e30;
  for (auto plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) {
    double prev_minx = minx_;
    double prev_miny = miny_;
    double prev_maxx = maxx_;
    double prev_maxy = maxy_;
    (*plugin)->UpdateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    count++;
    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) {
      ROS_WARN("Illegal bounds change. The offending layer is %s", (*plugin)->GetName().c_str());
    }
  }
  int x0, xn, y0, yn;
  costmap_.World2MapWithBoundary(minx_, miny_, x0, y0);
  costmap_.World2MapWithBoundary(maxx_, maxy_, xn, yn);
  x0 = std::max(0, x0);
  xn = std::min(int(costmap_.GetSizeXCell()), xn + 1);
  y0 = std::max(0, y0);
  yn = std::min(int(costmap_.GetSizeYCell()), yn + 1);
  if (xn < x0 || yn < y0) {
    return;
  }
  costmap_.ResetPartMap(x0, y0, xn, yn);
  for (auto plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) {
    (*plugin)->UpdateCosts(costmap_, x0, y0, xn, yn);
  }

  bx0_ = x0;
  bxn_ = xn;
  by0_ = y0;
  byn_ = yn;
  is_initialized_ = true;
}

void CostmapLayers::SetFootprint(const std::vector<geometry_msgs::Point> &footprint_spec) {
  footprint_ = footprint_spec;