src/lib/iterators/polygon_iterator.cpp
Go to the documentation of this file.
1 
4 /*****************************************************************************
5 ** Includes
6 *****************************************************************************/
7 
10 
11 
12 using namespace std;
13 
14 namespace cost_map {
15 
16 PolygonIterator::PolygonIterator(const cost_map::CostMap& gridMap, const cost_map::Polygon& polygon)
17  : polygon_(polygon)
18 {
19  mapLength_ = gridMap.getLength();
20  mapPosition_ = gridMap.getPosition();
21  resolution_ = gridMap.getResolution();
22  bufferSize_ = gridMap.getSize();
23  bufferStartIndex_ = gridMap.getStartIndex();
24  Index submapStartIndex;
25  Size submapBufferSize;
26  findSubmapParameters(polygon, submapStartIndex, submapBufferSize);
27  internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
28  if(!isInside()) ++(*this);
29 }
30 
32 {
33  polygon_ = other.polygon_;
35  mapLength_ = other.mapLength_;
36  mapPosition_ = other.mapPosition_;
37  resolution_ = other.resolution_;
38  bufferSize_ = other.bufferSize_;
40  return *this;
41 }
42 
44 {
45  return (internalIterator_ != other.internalIterator_);
46 }
47 
49 {
50  return *(*internalIterator_);
51 }
52 
54 {
55  ++(*internalIterator_);
56  if (internalIterator_->isPastEnd()) return *this;
57 
58  for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
59  if (isInside()) break;
60  }
61 
62  return *this;
63 }
64 
66 {
67  return internalIterator_->isPastEnd();
68 }
69 
71 {
72  Position position;
74  return polygon_.isInside(position);
75 }
76 
77 void PolygonIterator::findSubmapParameters(const cost_map::Polygon& polygon, Index& startIndex, Size& bufferSize) const
78 {
79  Position topLeft = polygon_.getVertices()[0];
80  Position bottomRight = topLeft;
81  for (const auto& vertex : polygon_.getVertices()) {
82  topLeft = topLeft.array().max(vertex.array());
83  bottomRight = bottomRight.array().min(vertex.array());
84  }
88  Index endIndex;
90  bufferSize = grid_map::getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
91 }
92 
93 } /* namespace cost_map */
94 
const Length & getLength() const
const Size & getSize() const
Length mapLength_
Map information needed to get position from iterator.
double getResolution() const
grid_map::Size Size
Definition: common.hpp:46
bool getIndexFromPosition(Index &index, const Position &position, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
void findSubmapParameters(const cost_map::Polygon &polygon, Index &startIndex, Size &bufferSize) const
Size getSubmapSizeFromCornerIndeces(const Index &topLeftIndex, const Index &bottomRightIndex, const Size &bufferSize, const Index &bufferStartIndex)
bool isInside(const Position &point) const
cost_map::Polygon polygon_
Polygon to iterate on.
bool getPosition(const Index &index, Position &position) const
grid_map::Index Index
Definition: common.hpp:45
bool operator!=(const PolygonIterator &other) const
grid_map::Position Position
Definition: common.hpp:41
PolygonIterator & operator=(const PolygonIterator &other)
std::shared_ptr< SubmapIterator > internalIterator_
Grid submap iterator.
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
const Index & getStartIndex() const
void boundPositionToRange(Position &position, const Length &mapLength, const Position &mapPosition)
const std::vector< Position > & getVertices() const


cost_map_core
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:03:41