GridMapVisual.hpp

Go to the documentation of this file.

/*
 * GridMapVisual.cpp
 *
 *  Created on: Aug 3, 2016
 *  Author: Philipp Krüsi, Péter Fankhauser
 *  Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include <OGRE/OgreMaterial.h>
#include <OGRE/OgreSharedPtr.h>

#include <grid_map_msgs/GridMap.h>
#include <grid_map_core/GridMap.hpp>

namespace Ogre {
class Vector3;
class Quaternion;
class ManualObject;
class ColourValue;
}  // namespace Ogre

namespace rviz {
class BillboardLine;
}

namespace grid_map_rviz_plugin {

// Visualizes a single grid_map_msgs::GridMap message.
class GridMapVisual {
 public:
  using MaskArray = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>;
  using ColorArray = Eigen::Array<Ogre::ColourValue, Eigen::Dynamic, Eigen::Dynamic>;
  using MatrixConstRef = Eigen::Ref<const grid_map::Matrix>;

  GridMapVisual(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode);
  virtual ~GridMapVisual();

  // Copy the grid map data to map_.
  void setMessage(const grid_map_msgs::GridMap::ConstPtr& msg);
  // Compute the visualization of map_.

  void computeVisualization(float alpha, bool showGridLines, bool flatTerrain, std::string heightLayer, bool flatColor, bool noColor,
                            Ogre::ColourValue meshColor, bool mapLayerColor, std::string colorLayer, std::string colorMap, bool useColorMap,
                            bool invertColorMap, Ogre::ColourValue minColor, Ogre::ColourValue maxColor, bool autocomputeIntensity,
                            float minIntensity, float maxIntensity, float gridLineThickness, int gridCellDecimation);

  // Set the coordinate frame pose.
  void setFramePosition(const Ogre::Vector3& position);
  void setFrameOrientation(const Ogre::Quaternion& orientation);

  // Get grid map layer names.
  std::vector<std::string> getLayerNames();

 private:
  enum class ColoringMethod { FLAT, COLOR_LAYER, INTENSITY_LAYER_MANUAL, INTENSITY_LAYER_COLORMAP, INTENSITY_LAYER_INVERTED_COLORMAP };

  Ogre::SceneNode* frameNode_;
  Ogre::SceneManager* sceneManager_;

  // ManualObject for mesh display.
  Ogre::ManualObject* manualObject_;
  Ogre::MaterialPtr material_;
  std::string materialName_;

  // Lines for mesh.
  boost::shared_ptr<rviz::BillboardLine> meshLines_;

  // Grid map.
  grid_map::GridMap map_;

  // Helper methods.
  bool haveMap_;

  // Warning logs
  static constexpr double warningMessageThrottlePeriod_{10.0};

  void initializeAndBeginManualObject(size_t nVertices);

  ColorArray computeColorValues(MatrixConstRef heightData, MatrixConstRef colorData,
                                ColoringMethod coloringMethod, std::string colorMap, Ogre::ColourValue flatColor,
                                double minIntensity, double maxIntensity, bool autocomputeIntensity, Ogre::ColourValue minColor,
                                Ogre::ColourValue maxColor);

  void initializeMeshLines(size_t cols, size_t rows, double resolution, double alpha, double lineWidth);

  MaskArray computeIsValidMask(std::vector<std::string> basicLayers);

  void printMissingBasicLayers(const std::vector<std::string>& basicLayers) const;

  static void normalizeIntensity(float& intensity, float minIntensity, float maxIntensity);

  static Ogre::ColourValue getRainbowColor(float intensity);

  Ogre::ColourValue getInterpolatedColor(float intensity, Ogre::ColourValue minColor, Ogre::ColourValue maxColor);

  std::vector<Ogre::Vector3> computeMeshLineVertices(int i, int j, int gridCellDecimation, bool isNthRow, bool isNthCol, bool isLastRow,
                                                     bool isLastCol, double resolution, const grid_map::Position& topLeft,
                                                     const Eigen::ArrayXXf& heightOrFlatData, const MaskArray& isValid) const;
};

}  // namespace grid_map_rviz_plugin