GridMap GridMap::getSubmap(const Eigen::Vector2d& position, const Eigen::Array2d& length, Eigen::Array2i& indexInSubmap, bool& isSuccess)
{
// Submap the generate.
GridMap submap(types_);
submap.setClearTypes(clearTypes_);
submap.setTimestamp(timestamp_);
submap.setFrameId(frameId_);
// Get submap geometric information.
Array2i topLeftIndex;
Array2i submapBufferSize;
Eigen::Vector2d submapPosition;
Array2d submapLength;
if (!getSubmapInformation(topLeftIndex, submapBufferSize, submapPosition, submapLength, indexInSubmap, position,
length, length_, position_, resolution_, bufferSize_, bufferStartIndex_)) {
isSuccess = false;
return GridMap(types_);
}
submap.setGeometry(submapLength, resolution_, submapPosition);
submap.bufferStartIndex_.setZero(); // Because of the way we copy the data below.
// Copy data.
std::vector<Eigen::Array2i> submapIndeces;
std::vector<Eigen::Array2i> submapSizes;
if (!getBufferRegionsForSubmap(submapIndeces, submapSizes, topLeftIndex, submap.bufferSize_, bufferSize_, bufferStartIndex_)) {
cout << "Cannot access submap of this size." << endl;
isSuccess = false;
return GridMap(types_);
}
for (auto& data : data_) {
submap.data_[data.first].topLeftCorner (submapSizes[0](0), submapSizes[0](1)) = data.second.block(submapIndeces[0](0), submapIndeces[0](1), submapSizes[0](0), submapSizes[0](1));
submap.data_[data.first].topRightCorner (submapSizes[1](0), submapSizes[1](1)) = data.second.block(submapIndeces[1](0), submapIndeces[1](1), submapSizes[1](0), submapSizes[1](1));
submap.data_[data.first].bottomLeftCorner (submapSizes[2](0), submapSizes[2](1)) = data.second.block(submapIndeces[2](0), submapIndeces[2](1), submapSizes[2](0), submapSizes[2](1));
submap.data_[data.first].bottomRightCorner(submapSizes[3](0), submapSizes[3](1)) = data.second.block(submapIndeces[3](0), submapIndeces[3](1), submapSizes[3](0), submapSizes[3](1));
}
isSuccess = true;
return submap;
}
bool ElevationMap::fuseArea(const Eigen::Vector2d& position, const Eigen::Array2d& length)
{
ROS_DEBUG("Requested to fuse an area of the elevation map with center at (%f, %f) and side lengths (%f, %f)",
position[0], position[1], length[0], length[1]);
Index topLeftIndex;
Index submapBufferSize;
// These parameters are not used in this function.
Position submapPosition;
Length submapLength;
Index requestedIndexInSubmap;
boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);
getSubmapInformation(topLeftIndex, submapBufferSize, submapPosition, submapLength,
requestedIndexInSubmap, position, length, rawMap_.getLength(),
rawMap_.getPosition(), rawMap_.getResolution(), rawMap_.getSize(),
rawMap_.getStartIndex());
return fuse(topLeftIndex, submapBufferSize);
}
