bool LineIterator::getIndexLimitedToMapRange(const grid_map::GridMap& gridMap, const Position& start, const Position& end, Index& index) { Position newStart = start; Vector direction = (end - start).normalized(); while (!gridMap.getIndex(newStart, index)) { newStart += (gridMap.getResolution() - std::numeric_limits<double>::epsilon()) * direction; if ((end - newStart).norm() < gridMap.getResolution() - std::numeric_limits<double>::epsilon()) return false; } return true; }
SpiralIterator::SpiralIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& center, const double radius) : center_(center), radius_(radius), distance_(0) { radiusSquare_ = radius_ * radius_; mapLength_ = gridMap.getLength(); mapPosition_ = gridMap.getPosition(); resolution_ = gridMap.getResolution(); bufferSize_ = gridMap.getSize(); gridMap.getIndex(center_, indexCenter_); nRings_ = std::ceil(radius_ / resolution_); if (checkIfIndexWithinRange(indexCenter_, bufferSize_)) pointsRing_.push_back(indexCenter_); else generateRing(); }