Method getClosestPositionInMap

grid_map_core/src/GridMap.cpp:698–732

}

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

  if (isInside(position)) {
    return position;
  }

  Position positionInMap = position;

  // Find edges.
  const double halfLengthX = length_.x() * 0.5;
  const double halfLengthY = length_.y() * 0.5;
  const Position3 topLeftCorner(position_.x() + halfLengthX, position_.y() + halfLengthY, 0.0);
  const Position3 topRightCorner(position_.x() + halfLengthX, position_.y() - halfLengthY, 0.0);
  const Position3 bottomLeftCorner(position_.x() - halfLengthX, position_.y() + halfLengthY, 0.0);
  const Position3 bottomRightCorner(position_.x() - halfLengthX, position_.y() - halfLengthY, 0.0);

  // Find constraints.
  const double maxX = topRightCorner.x();
  const double minX = bottomRightCorner.x();
  const double maxY = bottomLeftCorner.y();
  const double minY = bottomRightCorner.y();

  // Clip to box constraints and correct for indexing precision.
  // Points on the border can lead to invalid indices because the cells represent half open intervals, i.e. [...).
  positionInMap.x() = std::fmin(positionInMap.x(), maxX - std::numeric_limits<double>::epsilon());
  positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits<double>::epsilon());

  positionInMap.x() = std::fmax(positionInMap.x(), minX + std::numeric_limits<double>::epsilon());
  positionInMap.y() = std::fmax(positionInMap.y(), minY + std::numeric_limits<double>::epsilon());

  return positionInMap;
}

void GridMap::clear(const std::string& layer) {
  try {