Method convertToDefaultStartIndex

grid_map_core/src/GridMap.cpp:668–696

}

void GridMap::convertToDefaultStartIndex() {
  if (isDefaultStartIndex()) return;

  std::vector<BufferRegion> bufferRegions;
  if (!getBufferRegionsForSubmap(bufferRegions, startIndex_, size_, size_, startIndex_)) {
    throw std::out_of_range("Cannot access submap of this size.");
  }

  for (auto& data : data_) {
    auto tempData(data.second);
    for (const auto& bufferRegion : bufferRegions) {
      Index index = bufferRegion.getStartIndex();
      Size size = bufferRegion.getSize();

      if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopLeft) {
        tempData.topLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::TopRight) {
        tempData.topRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomLeft) {
        tempData.bottomLeftCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      } else if (bufferRegion.getQuadrant() == BufferRegion::Quadrant::BottomRight) {
        tempData.bottomRightCorner(size(0), size(1)) = data.second.block(index(0), index(1), size(0), size(1));
      }
    }
    data.second = tempData;
  }

  startIndex_.setZero();
}

Position GridMap::getClosestPositionInMap(const Position& position) const {
  if (getSize().x() < 1u || getSize().y() < 1u) {