| 617 | } |
| 618 | |
| 619 | void ElevationMap::underlyingMapCallback(const grid_map_msgs::GridMap& underlyingMap) |
| 620 | { |
| 621 | ROS_INFO("Updating underlying map."); |
| 622 | GridMapRosConverter::fromMessage(underlyingMap, underlyingMap_); |
| 623 | if (underlyingMap_.getFrameId() != rawMap_.getFrameId()) { |
| 624 | ROS_ERROR_STREAM("The underlying map does not have the same map frame ('" <<underlyingMap_.getFrameId() |
| 625 | << "') as the elevation map ('" << rawMap_.getFrameId() << "')."); |
| 626 | return; |
| 627 | } |
| 628 | if (!underlyingMap_.exists("elevation")) { |
| 629 | ROS_ERROR_STREAM("The underlying map does not have an 'elevation' layer."); |
| 630 | return; |
| 631 | } |
| 632 | if (!underlyingMap_.exists("variance")) underlyingMap_.add("variance", minVariance_); |
| 633 | if (!underlyingMap_.exists("horizontal_variance_x")) underlyingMap_.add("horizontal_variance_x", minHorizontalVariance_); |
| 634 | if (!underlyingMap_.exists("horizontal_variance_y")) underlyingMap_.add("horizontal_variance_y", minHorizontalVariance_); |
| 635 | if (!underlyingMap_.exists("color")) underlyingMap_.add("color", 0.0); |
| 636 | underlyingMap_.setBasicLayers(rawMap_.getBasicLayers()); |
| 637 | hasUnderlyingMap_ = true; |
| 638 | rawMap_.addDataFrom(underlyingMap_, false, false, true); |
| 639 | } |
| 640 | |
| 641 | float ElevationMap::cumulativeDistributionFunction(float x, float mean, float standardDeviation) |
| 642 | { |