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); } }