grid_map_rviz_plugin: GridMapVisual.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * GridMapVisual.cpp
00003  *
00004  *  Created on: Aug 3, 2016
00005  *  Author: Philipp Krüsi, Péter Fankhauser
00006  *  Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #include <rviz/uniform_string_stream.h>
00010 #include <OGRE/OgreMaterialManager.h>
00011 #include <OGRE/OgreTextureManager.h>
00012 #include <OGRE/OgreTechnique.h>
00013 
00014 #include <OGRE/OgreVector3.h>
00015 #include <OGRE/OgreSceneNode.h>
00016 #include <OGRE/OgreSceneManager.h>
00017 #include <OGRE/OgreManualObject.h>
00018 
00019 #include <rviz/ogre_helpers/billboard_line.h>
00020 
00021 #include <grid_map_ros/grid_map_ros.hpp>
00022 #include <grid_map_core/GridMapMath.hpp>
00023 #include "grid_map_rviz_plugin/GridMapVisual.hpp"
00024 
00025 namespace grid_map_rviz_plugin {
00026 
00027 GridMapVisual::GridMapVisual(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode)
00028     : manualObject_(0),
00029       haveMap_(false)
00030 {
00031   sceneManager_ = sceneManager;
00032   frameNode_ = parentNode->createChildSceneNode();
00033 
00034   // Create BillboardLine object.
00035   meshLines_.reset(new rviz::BillboardLine(sceneManager_, frameNode_));
00036 }
00037 
00038 GridMapVisual::~GridMapVisual()
00039 {
00040   // Destroy the ManualObject.
00041   sceneManager_->destroyManualObject(manualObject_);
00042   material_->unload();
00043   Ogre::MaterialManager::getSingleton().remove(material_->getName());
00044 
00045   // Destroy the frame node.
00046   sceneManager_->destroySceneNode(frameNode_);
00047 }
00048 
00049 void GridMapVisual::setMessage(const grid_map_msgs::GridMap::ConstPtr& msg)
00050 {
00051   // Convert grid map message.
00052   grid_map::GridMapRosConverter::fromMessage(*msg, map_);
00053   haveMap_ = true;
00054 }
00055 
00056 void GridMapVisual::computeVisualization(float alpha, bool showGridLines, bool flatTerrain, std::string heightLayer,
00057                                          bool flatColor, bool noColor, Ogre::ColourValue meshColor, bool mapLayerColor,
00058                                          std::string colorLayer, bool useRainbow, bool invertRainbow,
00059                                          Ogre::ColourValue minColor, Ogre::ColourValue maxColor,
00060                                          bool autocomputeIntensity, float minIntensity, float maxIntensity)
00061 {
00062   if (!haveMap_) {
00063     ROS_DEBUG("Unable to visualize grid map, no map data. Use setMessage() first!");
00064     return;
00065   }
00066 
00067   // Get list of layers and check if the requested ones are present.
00068   std::vector<std::string> layerNames = map_.getLayers();
00069   if (layerNames.size() < 1) {
00070     ROS_DEBUG("Unable to visualize grid map, map must contain at least one layer.");
00071     return;
00072   }
00073   if ((!flatTerrain && !map_.exists(heightLayer)) || (!noColor && !flatColor && !map_.exists(colorLayer))) {
00074     ROS_DEBUG("Unable to visualize grid map, requested layer(s) not available.");
00075     return;
00076   }
00077 
00078   // Convert to simple format, makes things easier.
00079   map_.convertToDefaultStartIndex();
00080 
00081   // Basic grid map data.
00082   const size_t rows = map_.getSize()(0);
00083   const size_t cols = map_.getSize()(1);
00084   if (rows < 2 || cols < 2) {
00085     ROS_DEBUG("GridMap has not enough cells.");
00086     return;
00087   }
00088   const double resolution = map_.getResolution();
00089   const grid_map::Matrix& heightData = map_[flatTerrain ? layerNames[0] : heightLayer];
00090   const grid_map::Matrix& colorData = map_[flatColor ? layerNames[0] : colorLayer];
00091 
00092   // initialize ManualObject
00093   if (!manualObject_) {
00094     static uint32_t count = 0;
00095     rviz::UniformStringStream ss;
00096     ss << "Mesh" << count++;
00097     manualObject_ = sceneManager_->createManualObject(ss.str());
00098     frameNode_->attachObject(manualObject_);
00099 
00100     ss << "Material";
00101     materialName_ = ss.str();
00102     material_ = Ogre::MaterialManager::getSingleton().create(materialName_, "rviz");
00103     material_->setReceiveShadows(false);
00104     material_->getTechnique(0)->setLightingEnabled(true);
00105     material_->setCullingMode(Ogre::CULL_NONE);
00106   }
00107 
00108   manualObject_->clear();
00109   size_t nVertices = 4 + 6 * (cols * rows - cols - rows);
00110   manualObject_->estimateVertexCount(nVertices);
00111   manualObject_->begin(materialName_, Ogre::RenderOperation::OT_TRIANGLE_LIST);
00112 
00113   meshLines_->clear();
00114   if (showGridLines) {
00115     meshLines_->setColor(0.0, 0.0, 0.0, alpha);
00116     meshLines_->setLineWidth(resolution / 10.0);
00117     meshLines_->setMaxPointsPerLine(2);
00118     // In the algorithm below, we have to account for max. 4 lines per cell.
00119     size_t nLines = 2 * (rows * (cols - 1) + cols * (rows - 1));
00120     meshLines_->setNumLines(nLines);
00121   }
00122 
00123   // Determine max and min intensity.
00124   if (autocomputeIntensity && !flatColor && !mapLayerColor) {
00125     minIntensity = colorData.minCoeffOfFinites();
00126     maxIntensity = colorData.maxCoeffOfFinites();
00127   }
00128 
00129   if (!map_.hasBasicLayers()) map_.setBasicLayers({heightLayer});
00130 
00131   // Plot mesh.
00132   for (size_t i = 0; i < rows - 1; ++i) {
00133     for (size_t j = 0; j < cols - 1; ++j) {
00134       std::vector<Ogre::Vector3> vertices;
00135       std::vector<Ogre::ColourValue> colors;
00136       for (size_t k = 0; k < 2; k++) {
00137         for (size_t l = 0; l < 2; l++) {
00138           grid_map::Position position;
00139           grid_map::Index index(i + k, j + l);
00140           if (!map_.isValid(index)) continue;
00141 
00142           map_.getPosition(index, position);
00143           float height = heightData(index(0), index(1));
00144           vertices.push_back(Ogre::Vector3(position(0), position(1), flatTerrain ? 0.0 : height));
00145           if (!flatColor) {
00146             float color = colorData(index(0), index(1));
00147             Ogre::ColourValue colorValue;
00148             if (mapLayerColor) {
00149               Eigen::Vector3f colorVectorRGB;
00150               grid_map::colorValueToVector(color, colorVectorRGB);
00151               colorValue = Ogre::ColourValue(colorVectorRGB(0), colorVectorRGB(1), colorVectorRGB(2));
00152             } else {
00153               normalizeIntensity(color, minIntensity, maxIntensity);
00154               colorValue = useRainbow ? (invertRainbow ? getRainbowColor(1.0f - color)
00155                                                        : getRainbowColor(color))
00156                                       : getInterpolatedColor(color, minColor, maxColor);
00157             }
00158             colors.push_back(colorValue);
00159           }
00160         }
00161       }
00162 
00163       // Plot triangles if we have enough vertices.
00164       if (vertices.size() > 2) {
00165         Ogre::Vector3 normal = vertices.size() == 4
00166                                ? (vertices[3] - vertices[0]).crossProduct(vertices[2] -vertices[1])
00167                                : (vertices[2] - vertices[1]).crossProduct(vertices[1] - vertices[0]);
00168         normal.normalise();
00169         // Create one or two triangles from the vertices depending on how many vertices we have.
00170         if (!noColor) {
00171           for (size_t m = 1; m < vertices.size() - 1; m++) {
00172             manualObject_->position(vertices[m-1]);
00173             manualObject_->normal(normal);
00174             Ogre::ColourValue color = flatColor ? meshColor : colors[m-1];
00175             manualObject_->colour(color.r, color.g, color.b, alpha);
00176 
00177             manualObject_->position(vertices[m]);
00178             manualObject_->normal(normal);
00179             color = flatColor ? meshColor : colors[m];
00180             manualObject_->colour(color.r, color.g, color.b, alpha);
00181 
00182             manualObject_->position(vertices[m+1]);
00183             manualObject_->normal(normal);
00184             color = flatColor ? meshColor : colors[m+1];
00185             manualObject_->colour(color.r, color.g, color.b, alpha);
00186           }
00187         }
00188 
00189         // plot grid lines
00190         if (showGridLines) {
00191           meshLines_->addPoint(vertices[0]);
00192           meshLines_->addPoint(vertices[1]);
00193           meshLines_->newLine();
00194 
00195           if (vertices.size() == 3) {
00196             meshLines_->addPoint(vertices[1]);
00197             meshLines_->addPoint(vertices[2]);
00198             meshLines_->newLine();
00199           } else {
00200             meshLines_->addPoint(vertices[1]);
00201             meshLines_->addPoint(vertices[3]);
00202             meshLines_->newLine();
00203 
00204             meshLines_->addPoint(vertices[3]);
00205             meshLines_->addPoint(vertices[2]);
00206             meshLines_->newLine();
00207           }
00208 
00209           meshLines_->addPoint(vertices[2]);
00210           meshLines_->addPoint(vertices[0]);
00211           meshLines_->newLine();
00212         }
00213       }
00214     }
00215   }
00216 
00217   manualObject_->end();
00218   material_->getTechnique(0)->setLightingEnabled(false);
00219 
00220   if (alpha < 0.9998) {
00221     material_->getTechnique(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
00222     material_->getTechnique(0)->setDepthWriteEnabled(false);
00223   } else {
00224     material_->getTechnique(0)->setSceneBlending(Ogre::SBT_REPLACE);
00225     material_->getTechnique(0)->setDepthWriteEnabled(true);
00226   }
00227 }
00228 
00229 void GridMapVisual::setFramePosition(const Ogre::Vector3& position)
00230 {
00231   frameNode_->setPosition(position);
00232 }
00233 
00234 void GridMapVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
00235 {
00236   frameNode_->setOrientation(orientation);
00237 }
00238 
00239 std::vector<std::string> GridMapVisual::getLayerNames()
00240 {
00241   return map_.getLayers();
00242 }
00243 
00244 // Compute intensity value in the interval [0,1].
00245 void GridMapVisual::normalizeIntensity(float& intensity, float min_intensity, float max_intensity)
00246 {
00247   intensity = std::min(intensity, max_intensity);
00248   intensity = std::max(intensity, min_intensity);
00249   intensity = (intensity - min_intensity) / (max_intensity - min_intensity);
00250 }
00251 
00252 // Copied from rviz/src/rviz/default_plugin/point_cloud_transformers.cpp.
00253 Ogre::ColourValue GridMapVisual::getRainbowColor(float intensity)
00254 {
00255   intensity = std::min(intensity, 1.0f);
00256   intensity = std::max(intensity, 0.0f);
00257 
00258   float h = intensity * 5.0f + 1.0f;
00259   int i = floor(h);
00260   float f = h - i;
00261   if (!(i & 1)) f = 1 - f;  // if i is even
00262   float n = 1 - f;
00263 
00264   Ogre::ColourValue color;
00265   if (i <= 1) color[0] = n, color[1] = 0, color[2] = 1;
00266   else if (i == 2) color[0] = 0, color[1] = n, color[2] = 1;
00267   else if (i == 3) color[0] = 0, color[1] = 1, color[2] = n;
00268   else if (i == 4) color[0] = n, color[1] = 1, color[2] = 0;
00269   else if (i >= 5) color[0] = 1, color[1] = n, color[2] = 0;
00270 
00271   return color;
00272 }
00273 
00274 // Get interpolated color value.
00275 Ogre::ColourValue GridMapVisual::getInterpolatedColor(float intensity, Ogre::ColourValue min_color,
00276                                                       Ogre::ColourValue max_color)
00277 {
00278   intensity = std::min(intensity, 1.0f);
00279   intensity = std::max(intensity, 0.0f);
00280 
00281   Ogre::ColourValue color;
00282   color.r = intensity * (max_color.r - min_color.r) + min_color.r;
00283   color.g = intensity * (max_color.g - min_color.g) + min_color.g;
00284   color.b = intensity * (max_color.b - min_color.b) + min_color.b;
00285 
00286   return color;
00287 }
00288 
00289 }  // namespace