bool ElevationMap::publishFusedElevationMap()
{
if (!hasFusedMapSubscribers()) return false;
boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);
GridMap fusedMapCopy = fusedMap_;
scopedLock.unlock();
fusedMapCopy.add("uncertainty_range", fusedMapCopy.get("upper_bound") - fusedMapCopy.get("lower_bound"));
grid_map_msgs::GridMap message;
GridMapRosConverter::toMessage(fusedMapCopy, message);
elevationMapFusedPublisher_.publish(message);
ROS_DEBUG("Elevation map (fused) has been published.");
return true;
}
void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer,
const double heightClearance)
{
data_.clear();
resolution_ = gridMap.getResolution();
position_ = gridMap.getPosition();
size_ = gridMap.getSize();
Matrix map = gridMap.get(layer); // Copy!
float minHeight = map.minCoeffOfFinites();
if (!std::isfinite(minHeight)) minHeight = lowestHeight_;
float maxHeight = map.maxCoeffOfFinites();
if (!std::isfinite(maxHeight)) maxHeight = lowestHeight_;
const float valueForEmptyCells = lowestHeight_; // maxHeight, minHeight (TODO Make this an option).
for (size_t i = 0; i < map.size(); ++i) {
if (std::isnan(map(i))) map(i) = valueForEmptyCells;
}
// Height range of the signed distance field is higher than the max height.
maxHeight += heightClearance;
Matrix sdfElevationAbove = Matrix::Ones(map.rows(), map.cols()) * maxDistance_;
Matrix sdfLayer = Matrix::Zero(map.rows(), map.cols());
std::vector<Matrix> sdf;
zIndexStartHeight_ = minHeight;
// Calculate signed distance field from bottom.
for (float h = minHeight; h < maxHeight; h += resolution_) {
Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleFreeField = map.array() < h;
Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleField = obstacleFreeField.array() < 1;
Matrix sdfObstacle = getPlanarSignedDistanceField(obstacleField);
Matrix sdfObstacleFree = getPlanarSignedDistanceField(obstacleFreeField);
Matrix sdf2d;
// If 2d sdfObstacleFree calculation failed, neglect this SDF
// to avoid extreme small distances (-INF).
if ((sdfObstacleFree.array() >= INF).any()) sdf2d = sdfObstacle;
else sdf2d = sdfObstacle - sdfObstacleFree;
sdf2d *= resolution_;
for (size_t i = 0; i < sdfElevationAbove.size(); ++i) {
if(sdfElevationAbove(i) == maxDistance_ && map(i) <= h) sdfElevationAbove(i) = h - map(i);
else if(sdfElevationAbove(i) != maxDistance_ && map(i) <= h) sdfElevationAbove(i) = sdfLayer(i) + resolution_;
if (sdf2d(i) == 0) sdfLayer(i) = h - map(i);
else if (sdf2d(i) < 0) sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h));
else sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i));
}
data_.push_back(sdfLayer);
}
}
