Method ClearLayer

roborts_costmap/src/costmap_interface.cpp:444–473

}

void CostmapInterface::ClearLayer(CostmapLayer *costmap_layer_ptr, double pose_x, double pose_y) {
  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_layer_ptr->GetMutex()));
  double reset_distance = 0.1;
  double start_point_x = pose_x - reset_distance / 2;
  double start_point_y = pose_y - reset_distance / 2;
  double end_point_x = start_point_x + reset_distance;
  double end_point_y = start_point_y + reset_distance;

  int start_x, start_y, end_x, end_y;
  costmap_layer_ptr->World2MapNoBoundary(start_point_x, start_point_y, start_x, start_y);
  costmap_layer_ptr->World2MapNoBoundary(end_point_x, end_point_y, end_x, end_y);

  unsigned char *grid = costmap_layer_ptr->GetCharMap();
  for (int x = 0; x < (int) costmap_layer_ptr->GetSizeXCell(); x++) {
    bool xrange = x > start_x && x < end_x;

    for (int y = 0; y < (int) costmap_layer_ptr->GetSizeYCell(); y++) {
      if (xrange && y > start_y && y < end_y)
        continue;
      int index = costmap_layer_ptr->GetIndex(x, y);
      if (grid[index] != NO_INFORMATION) {
        grid[index] = NO_INFORMATION;
      }
    }
  }

  double ox = costmap_layer_ptr->GetOriginX(), oy = costmap_layer_ptr->GetOriginY();
  double width = costmap_layer_ptr->GetSizeXWorld(), height = costmap_layer_ptr->GetSizeYWorld();
  costmap_layer_ptr->AddExtraBounds(ox, oy, ox + width, oy + height);
}
} //namespace roborts_costmap