Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include <cost_map_core/iterators/spiral_iterator.hpp>
00010 #include <grid_map_core/GridMapMath.hpp>
00011 #include <cmath>
00012
00013 using namespace std;
00014
00015 namespace cost_map {
00016
00017 SpiralIterator::SpiralIterator(const cost_map::CostMap& gridMap, const Eigen::Vector2d& center,
00018 const double radius)
00019 : center_(center),
00020 radius_(radius),
00021 distance_(0)
00022 {
00023 radiusSquare_ = radius_ * radius_;
00024 mapLength_ = gridMap.getLength();
00025 mapPosition_ = gridMap.getPosition();
00026 resolution_ = gridMap.getResolution();
00027 bufferSize_ = gridMap.getSize();
00028 gridMap.getIndex(center_, indexCenter_);
00029 nRings_ = std::ceil(radius_ / resolution_);
00030 if (grid_map::checkIfIndexInRange(indexCenter_, bufferSize_)) pointsRing_.push_back(indexCenter_);
00031 else generateRing();
00032 }
00033
00034 SpiralIterator& SpiralIterator::operator =(const SpiralIterator& other)
00035 {
00036 center_ = other.center_;
00037 indexCenter_ = other.indexCenter_;
00038 radius_ = other.radius_;
00039 radiusSquare_ = other.radiusSquare_;
00040 nRings_ = other.nRings_;
00041 distance_ = other.distance_;
00042 pointsRing_ = other.pointsRing_;
00043 mapLength_ = other.mapLength_;
00044 mapPosition_ = other.mapPosition_;
00045 resolution_ = other.resolution_;
00046 bufferSize_ = other.bufferSize_;
00047 return *this;
00048 }
00049
00050 bool SpiralIterator::operator !=(const SpiralIterator& other) const
00051 {
00052 return (pointsRing_.back() != pointsRing_.back()).any();
00053 }
00054
00055 const Eigen::Array2i& SpiralIterator::operator *() const
00056 {
00057 return pointsRing_.back();
00058 }
00059
00060 SpiralIterator& SpiralIterator::operator ++()
00061 {
00062 pointsRing_.pop_back();
00063 if (pointsRing_.empty() && !isPastEnd()) generateRing();
00064 return *this;
00065 }
00066
00067 bool SpiralIterator::isPastEnd() const
00068 {
00069 return (distance_ == nRings_ && pointsRing_.empty());
00070 }
00071
00072 bool SpiralIterator::isInside(const Index index) const
00073 {
00074 Eigen::Vector2d position;
00075 grid_map::getPositionFromIndex(position, index, mapLength_, mapPosition_, resolution_, bufferSize_);
00076 double squareNorm = (position - center_).array().square().sum();
00077 return (squareNorm <= radiusSquare_);
00078 }
00079
00080 void SpiralIterator::generateRing()
00081 {
00082 distance_++;
00083 Index point(distance_, 0);
00084 Index pointInMap;
00085 Index normal;
00086 do {
00087 pointInMap.x() = point.x() + indexCenter_.x();
00088 pointInMap.y() = point.y() + indexCenter_.y();
00089 if (grid_map::checkIfIndexInRange(pointInMap, bufferSize_)) {
00090 if (distance_ == nRings_ || distance_ == nRings_ - 1) {
00091 if (isInside(pointInMap))
00092 pointsRing_.push_back(pointInMap);
00093 } else {
00094 pointsRing_.push_back(pointInMap);
00095 }
00096 }
00097 normal.x() = -signum(point.y());
00098 normal.y() = signum(point.x());
00099 if (normal.x() != 0
00100 && (int) Vector(point.x() + normal.x(), point.y()).norm() == distance_)
00101 point.x() += normal.x();
00102 else if (normal.y() != 0
00103 && (int) Vector(point.x(), point.y() + normal.y()).norm() == distance_)
00104 point.y() += normal.y();
00105 else {
00106 point.x() += normal.x();
00107 point.y() += normal.y();
00108 }
00109 } while (point.x() != distance_ || point.y() != 0);
00110 }
00111
00112 double SpiralIterator::getCurrentRadius() const
00113 {
00114 Index radius = *(*this) - indexCenter_;
00115 return radius.matrix().norm() * resolution_;
00116 }
00117
00118 }
00119