Skip to content

Commit

Permalink
Changes from feedback on ROS Book chapter.
Browse files Browse the repository at this point in the history
  • Loading branch information
Peter Fankhauser committed Jun 30, 2015
1 parent 7870e62 commit ee45582
Show file tree
Hide file tree
Showing 20 changed files with 120 additions and 109 deletions.
8 changes: 4 additions & 4 deletions grid_map/src/GridMapRosConverter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -211,7 +211,7 @@ void GridMapRosConverter::toOccupancyGrid(const grid_map::GridMap& gridMap,
const float cellMax = 100;
const float cellRange = cellMax - cellMin;

for (GridMapIterator iterator(gridMap); !iterator.isPassedEnd(); ++iterator) {
for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
float value = (gridMap.at(layer, *iterator) - dataMin) / (dataMax - dataMin);
if (isnan(value))
value = -1;
Expand All @@ -233,7 +233,7 @@ void GridMapRosConverter::toGridCells(const grid_map::GridMap& gridMap, const st
gridCells.cell_width = gridMap.getResolution();
gridCells.cell_height = gridMap.getResolution();

for (GridMapIterator iterator(gridMap); !iterator.isPassedEnd(); ++iterator) {
for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
if (!gridMap.isValid(*iterator, layer)) continue;
if (gridMap.at(layer, *iterator) >= lowerThreshold
&& gridMap.at(layer, *iterator) <= upperThreshold) {
Expand Down Expand Up @@ -310,7 +310,7 @@ bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image,
return false;
}

for (GridMapIterator iterator(gridMap); !iterator.isPassedEnd(); ++iterator) {
for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
// Set transparent values.
if (image.encoding == sensor_msgs::image_encodings::BGRA8) {
const auto& cvAlpha = cvPtrAlpha->image.at<cv::Vec4b>((*iterator)(0),
Expand Down Expand Up @@ -369,7 +369,7 @@ bool GridMapRosConverter::addColorLayerFromImage(const sensor_msgs::Image& image
return false;
}

for (GridMapIterator iterator(gridMap); !iterator.isPassedEnd(); ++iterator) {
for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
const auto& cvColor = cvPtr->image.at<cv::Vec3b>((*iterator)(0),
(*iterator)(1));
Eigen::Vector3i colorVector;
Expand Down
22 changes: 14 additions & 8 deletions grid_map_core/include/grid_map_core/GridMap.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ class GridMap
/*!
* Checks if data layer exists.
* @param layer the name of the layer.
* @return true if type exists, false otherwise.
* @return true if layer exists, false otherwise.
*/
bool exists(const std::string& layer) const;

Expand All @@ -90,7 +90,7 @@ class GridMap
const grid_map::Matrix& get(const std::string& layer) const;

/*!
* Returns the grid map data for a type as non-const. Use this method
* Returns the grid map data for a layer as non-const. Use this method
* with care!
* @param layer the name of the layer to be returned.
* @return grid map data.
Expand All @@ -107,7 +107,7 @@ class GridMap
const grid_map::Matrix& operator [](const std::string& layer) const;

/*!
* Returns the grid map data for a type as non-const. Use this method
* Returns the grid map data for a layer as non-const. Use this method
* with care!
* @param layer the name of the layer to be returned.
* @return grid map data.
Expand All @@ -132,7 +132,7 @@ class GridMap
* Set the basic layers that need to be valid for a cell to be considered as valid.
* Also, the basic layers are set to NAN when clearing the cells with `clear()`.
* By default the list of basic layers is empty.
* @param basicLayers the list of types that are the basic types of the map.
* @param basicLayers the list of layer that are the basic types of the map.
*/
void setBasicLayers(const std::vector<std::string>& basicLayers);

Expand Down Expand Up @@ -279,14 +279,20 @@ class GridMap
void move(const grid_map::Position& position);

/*!
* Clears all cells (set to NAN) for all types defined as basic type.
* Clears all types if no basic types are defined.
* Clears all cells (set to NAN) for a layer.
* @param layer the layer to be cleared.
*/
void clear(const std::string& layer);

/*!
* Clears all cells (set to NAN) for all basic layers.
* Header information (geometry etc.) remains valid.
*/
void clear();
void clearBasic();

/*!
* Clears all cells of all types of the grid map (less efficient than clear()).
* Clears all cells of all layers.
* If basic layers are used, clearBasic() is preferred as it is more efficient.
* Header information (geometry etc.) remains valid.
*/
void clearAll();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -60,10 +60,10 @@ class CircleIterator
CircleIterator& operator ++();

/*!
* Indicates if iterator is passed end.
* Indicates if iterator is past end.
* @return true if iterator is out of scope, false if end has not been reached.
*/
bool isPassedEnd() const;
bool isPastEnd() const;

private:

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,10 +66,10 @@ class GridMapIterator
GridMapIterator end() const;

/*!
* Indicates if iterator is passed end.
* Indicates if iterator is past end.
* @return true if iterator is out of scope, false if end has not been reached.
*/
bool isPassedEnd() const;
bool isPastEnd() const;

private:

Expand All @@ -86,7 +86,7 @@ class GridMapIterator
Index index_;

//! Is iterator out of scope.
bool isPassedEnd_;
bool isPastEnd_;
};

} /* namespace */
Original file line number Diff line number Diff line change
Expand Up @@ -66,10 +66,10 @@ class LineIterator
LineIterator& operator ++();

/*!
* Indicates if iterator is passed end.
* Indicates if iterator is past end.
* @return true if iterator is out of scope, false if end has not been reached.
*/
bool isPassedEnd() const;
bool isPastEnd() const;

private:

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -60,10 +60,10 @@ class PolygonIterator
PolygonIterator& operator ++();

/*!
* Indicates if iterator is passed end.
* Indicates if iterator is past end.
* @return true if iterator is out of scope, false if end has not been reached.
*/
bool isPassedEnd() const;
bool isPastEnd() const;

private:

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -77,10 +77,10 @@ class SubmapIterator
SubmapIterator end() const;

/*!
* Indicates if iterator is passed end.
* Indicates if iterator is past end.
* @return true if iterator is out of scope, false if end has not been reached.
*/
bool isPassedEnd() const;
bool isPastEnd() const;

private:

Expand All @@ -106,7 +106,7 @@ class SubmapIterator
Eigen::Array2i submapIndex_;

//! Is iterator out of scope.
bool isPassedEnd_;
bool isPastEnd_;
};

} /* namespace */
27 changes: 16 additions & 11 deletions grid_map_core/src/GridMap.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -312,7 +312,7 @@ void GridMap::move(const grid_map::Position& position)
if (indexShift(i) != 0) {
if (abs(indexShift(i)) >= getSize()(i)) {
// Entire map is dropped.
clear();
clearAll();
} else {
// Drop cells out of map.
int sign = (indexShift(i) > 0 ? 1 : -1);
Expand Down Expand Up @@ -406,14 +406,19 @@ const Eigen::Array2i& GridMap::getStartIndex() const
return startIndex_;
}

void GridMap::clear()
void GridMap::clear(const std::string& layer)
{
if (basicLayers_.empty()) {
clearAll();
return;
try {
data_.at(layer).setConstant(NAN);
} catch (const std::out_of_range& exception) {
throw std::out_of_range("GridMap::clear(...) : No map layer '" + layer + "' available.");
}
for (auto& key : basicLayers_) {
data_.at(key).setConstant(NAN);
}

void GridMap::clearBasic()
{
for (auto& layer : basicLayers_) {
clear(layer);
}
}

Expand All @@ -426,15 +431,15 @@ void GridMap::clearAll()

void GridMap::clearCols(unsigned int index, unsigned int nCols)
{
for (auto& type : basicLayers_) {
data_.at(type).block(index, 0, nCols, getSize()(1)).setConstant(NAN);
for (auto& layer : basicLayers_) {
data_.at(layer).block(index, 0, nCols, getSize()(1)).setConstant(NAN);
}
}

void GridMap::clearRows(unsigned int index, unsigned int nRows)
{
for (auto& type : basicLayers_) {
data_.at(type).block(0, index, getSize()(0), nRows).setConstant(NAN);
for (auto& layer : basicLayers_) {
data_.at(layer).block(0, index, getSize()(0), nRows).setConstant(NAN);
}
}

Expand Down
8 changes: 4 additions & 4 deletions grid_map_core/src/iterators/CircleIterator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,18 +58,18 @@ const Eigen::Array2i& CircleIterator::operator *() const
CircleIterator& CircleIterator::operator ++()
{
++(*internalIterator_);
if (internalIterator_->isPassedEnd()) return *this;
if (internalIterator_->isPastEnd()) return *this;

for ( ; !internalIterator_->isPassedEnd(); ++(*internalIterator_)) {
for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
if (isInside()) break;
}

return *this;
}

bool CircleIterator::isPassedEnd() const
bool CircleIterator::isPastEnd() const
{
return internalIterator_->isPassedEnd();
return internalIterator_->isPastEnd();
}

bool CircleIterator::isInside()
Expand Down
12 changes: 6 additions & 6 deletions grid_map_core/src/iterators/GridMapIterator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ GridMapIterator::GridMapIterator(const grid_map::GridMap& gridMap)
endIndex_ = startIndex_ + gridMap.getSize() - Eigen::Array2i::Ones();
mapIndexWithinRange(endIndex_, size_);
index_ = startIndex_;
isPassedEnd_ = false;
isPastEnd_ = false;
}

GridMapIterator::GridMapIterator(const GridMapIterator* other)
Expand All @@ -27,7 +27,7 @@ GridMapIterator::GridMapIterator(const GridMapIterator* other)
startIndex_ = other->startIndex_;
endIndex_ = other->endIndex_;
index_ = other->index_;
isPassedEnd_ = other->isPassedEnd_;
isPastEnd_ = other->isPastEnd_;
}

GridMapIterator& GridMapIterator::operator =(const GridMapIterator& other)
Expand All @@ -36,7 +36,7 @@ GridMapIterator& GridMapIterator::operator =(const GridMapIterator& other)
startIndex_ = other.startIndex_;
endIndex_ = other.endIndex_;
index_ = other.index_;
isPassedEnd_ = other.isPassedEnd_;
isPastEnd_ = other.isPastEnd_;
return *this;
}

Expand All @@ -52,7 +52,7 @@ const Eigen::Array2i& GridMapIterator::operator *() const

GridMapIterator& GridMapIterator::operator ++()
{
isPassedEnd_ = !incrementIndex(index_, size_, startIndex_);
isPastEnd_ = !incrementIndex(index_, size_, startIndex_);
return *this;
}

Expand All @@ -63,9 +63,9 @@ GridMapIterator GridMapIterator::end() const
return res;
}

bool GridMapIterator::isPassedEnd() const
bool GridMapIterator::isPastEnd() const
{
return isPassedEnd_;
return isPastEnd_;
}

} /* namespace grid_map */
2 changes: 1 addition & 1 deletion grid_map_core/src/iterators/LineIterator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ LineIterator& LineIterator::operator ++()
return *this;
}

bool LineIterator::isPassedEnd() const
bool LineIterator::isPastEnd() const
{
return iCell_ >= nCells_;
}
Expand Down
8 changes: 4 additions & 4 deletions grid_map_core/src/iterators/PolygonIterator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,18 +53,18 @@ const Eigen::Array2i& PolygonIterator::operator *() const
PolygonIterator& PolygonIterator::operator ++()
{
++(*internalIterator_);
if (internalIterator_->isPassedEnd()) return *this;
if (internalIterator_->isPastEnd()) return *this;

for ( ; !internalIterator_->isPassedEnd(); ++(*internalIterator_)) {
for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
if (isInside()) break;
}

return *this;
}

bool PolygonIterator::isPassedEnd() const
bool PolygonIterator::isPastEnd() const
{
return internalIterator_->isPassedEnd();
return internalIterator_->isPastEnd();
}

bool PolygonIterator::isInside()
Expand Down
12 changes: 6 additions & 6 deletions grid_map_core/src/iterators/SubmapIterator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ SubmapIterator::SubmapIterator(const grid_map::GridMap& gridMap,
submapEndIndex_ = submapStartIndex + submapSize - Eigen::Array2i::Ones();
mapIndexWithinRange(submapEndIndex_, submapBufferSize_);
submapIndex_.setZero();
isPassedEnd_ = false;
isPastEnd_ = false;
}

SubmapIterator::SubmapIterator(const SubmapIterator* other)
Expand All @@ -37,7 +37,7 @@ SubmapIterator::SubmapIterator(const SubmapIterator* other)
submapEndIndex_ = other->submapEndIndex_;
index_ = other->index_;
submapIndex_ = other->submapIndex_;
isPassedEnd_ = other->isPassedEnd_;
isPastEnd_ = other->isPastEnd_;
}

SubmapIterator& SubmapIterator::operator =(const SubmapIterator& other)
Expand All @@ -49,7 +49,7 @@ SubmapIterator& SubmapIterator::operator =(const SubmapIterator& other)
submapEndIndex_ = other.submapEndIndex_;
index_ = other.index_;
submapIndex_ = other.submapIndex_;
isPassedEnd_ = other.isPassedEnd_;
isPastEnd_ = other.isPastEnd_;
return *this;
}

Expand All @@ -70,7 +70,7 @@ const Eigen::Array2i& SubmapIterator::getSubmapIndex() const

SubmapIterator& SubmapIterator::operator ++()
{
isPassedEnd_ = !incrementIndexForSubmap(submapIndex_, index_, submapStartIndex_,
isPastEnd_ = !incrementIndexForSubmap(submapIndex_, index_, submapStartIndex_,
submapBufferSize_, bufferSize_, startIndex_);
return *this;
}
Expand All @@ -82,9 +82,9 @@ SubmapIterator SubmapIterator::end() const
return res;
}

bool SubmapIterator::isPassedEnd() const
bool SubmapIterator::isPastEnd() const
{
return isPassedEnd_;
return isPastEnd_;
}

} /* namespace grid_map */
Expand Down
Loading

0 comments on commit ee45582

Please sign in to comment.