| 49 | } |
| 50 | |
| 51 | void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool flatTerrain, std::string heightLayer, bool flatColor, |
| 52 | bool noColor, Ogre::ColourValue meshColor, bool mapLayerColor, std::string colorLayer, |
| 53 | bool useRainbow, bool invertRainbow, Ogre::ColourValue minColor, Ogre::ColourValue maxColor, |
| 54 | bool autocomputeIntensity, float minIntensity, float maxIntensity) { |
| 55 | const auto startTime = std::chrono::high_resolution_clock::now(); |
| 56 | if (!haveMap_) { |
| 57 | ROS_DEBUG("Unable to visualize grid map, no map data. Use setMessage() first!"); |
| 58 | return; |
| 59 | } |
| 60 | |
| 61 | // Get list of layers and check if the requested ones are present. |
| 62 | const std::vector<std::string> layerNames = map_.getLayers(); |
| 63 | if (layerNames.size() < 1) { |
| 64 | ROS_DEBUG("Unable to visualize grid map, map must contain at least one layer."); |
| 65 | return; |
| 66 | } |
| 67 | if ((!flatTerrain && !map_.exists(heightLayer)) || (!noColor && !flatColor && !map_.exists(colorLayer))) { |
| 68 | ROS_DEBUG("Unable to visualize grid map, requested layer(s) not available."); |
| 69 | return; |
| 70 | } |
| 71 | |
| 72 | // Convert to simple format, makes things easier. |
| 73 | map_.convertToDefaultStartIndex(); |
| 74 | |
| 75 | // Basic grid map data. |
| 76 | const size_t rows = map_.getSize()(0); |
| 77 | const size_t cols = map_.getSize()(1); |
| 78 | if (rows < 2 || cols < 2) { |
| 79 | ROS_DEBUG("GridMap has not enough cells."); |
| 80 | return; |
| 81 | } |
| 82 | const double resolution = map_.getResolution(); |
| 83 | const grid_map::Matrix& heightData = map_[flatTerrain ? layerNames[0] : heightLayer]; |
| 84 | const grid_map::Matrix& colorData = map_[flatColor ? layerNames[0] : colorLayer]; |
| 85 | |
| 86 | // Reset and begin the manualObject (mesh). |
| 87 | // For more information: https://www.ogre3d.org/docs/api/1.7/class_ogre_1_1_manual_object.html#details |
| 88 | const size_t nVertices = cols * rows; |
| 89 | initializeAndBeginManualObject(nVertices); |
| 90 | |
| 91 | // Reset the mesh lines. |
| 92 | meshLines_->clear(); |
| 93 | if (showGridLines) { |
| 94 | initializeMeshLines(cols, rows, resolution, alpha); |
| 95 | } |
| 96 | |
| 97 | // Compute a mask of valid cells. |
| 98 | auto basicLayers = map_.getBasicLayers(); |
| 99 | if (std::find(basicLayers.begin(), basicLayers.end(), heightLayer) == basicLayers.end()) { |
| 100 | basicLayers.emplace_back(heightLayer); |
| 101 | } |
| 102 | const MaskArray isValid = computeIsValidMask(basicLayers); |
| 103 | |
| 104 | // Compute the display heights for each cell. |
| 105 | Eigen::ArrayXXf heightOrFlatData; |
| 106 | if (flatTerrain) { |
| 107 | heightOrFlatData = Eigen::ArrayXXf::Zero(heightData.rows(), heightData.cols()); |
| 108 | } else { |
no test coverage detected