GridMapDisplay.cpp
Go to the documentation of this file.
00001 /*
00002  * GridMapDisplay.cpp
00003  *
00004  *  Created on: Aug 3, 2016
00005  *  Author: Philipp Krüsi, Péter Fankhauser
00006  *  Institute: ETH Zurich, Autonomous Systems Lab
00007  */
00008 
00009 #include "grid_map_rviz_plugin/GridMapVisual.hpp"
00010 #include "grid_map_rviz_plugin/GridMapDisplay.hpp"
00011 #include "grid_map_rviz_plugin/modified/frame_manager.h"
00012 
00013 #include <OGRE/OgreSceneNode.h>
00014 #include <OGRE/OgreSceneManager.h>
00015 
00016 #include <tf/transform_listener.h>
00017 
00018 #include <rviz/visualization_manager.h>
00019 #include <rviz/properties/bool_property.h>
00020 #include <rviz/properties/color_property.h>
00021 #include <rviz/properties/float_property.h>
00022 #include <rviz/properties/int_property.h>
00023 #include <rviz/properties/enum_property.h>
00024 #include <rviz/properties/editable_enum_property.h>
00025 
00026 namespace grid_map_rviz_plugin {
00027 
00028 GridMapDisplay::GridMapDisplay()
00029 {
00030   alphaProperty_ = new rviz::FloatProperty("Alpha", 1.0,
00031                                            "0 is fully transparent, 1.0 is fully opaque.", this,
00032                                            SLOT(updateVisualization()));
00033 
00034   historyLengthProperty_ = new rviz::IntProperty("History Length", 1,
00035                                                  "Number of prior grid maps to display.", this,
00036                                                  SLOT(updateHistoryLength()));
00037 
00038   showGridLinesProperty_ = new rviz::BoolProperty(
00039       "Show Grid Lines", true, "Whether to show the lines connecting the grid cells.", this,
00040       SLOT(updateVisualization()));
00041 
00042   heightModeProperty_ = new rviz::EnumProperty("Height Transformer", "GridMapLayer",
00043                                                "Select the transformer to use to set the height.",
00044                                                this, SLOT(updateHeightMode()));
00045   heightModeProperty_->addOption("Layer", 0);
00046   heightModeProperty_->addOption("Flat", 1);
00047 
00048   heightTransformerProperty_ = new rviz::EditableEnumProperty(
00049       "Height Layer", "elevation", "Select the grid map layer to compute the height.", this,
00050       SLOT(updateVisualization()));
00051 
00052   colorModeProperty_ = new rviz::EnumProperty("Color Transformer", "GridMapLayer",
00053                                               "Select the transformer to use to set the color.",
00054                                               this, SLOT(updateColorMode()));
00055   colorModeProperty_->addOption("IntensityLayer", 0);
00056   colorModeProperty_->addOption("ColorLayer", 1);
00057   colorModeProperty_->addOption("FlatColor", 2);
00058   colorModeProperty_->addOption("None", 3);
00059 
00060   colorTransformerProperty_ = new rviz::EditableEnumProperty(
00061       "Color Layer", "elevation", "Select the grid map layer to compute the color.", this,
00062       SLOT(updateVisualization()));
00063 
00064   colorProperty_ = new rviz::ColorProperty("Color", QColor(200, 200, 200),
00065                                            "Color to draw the mesh.", this,
00066                                            SLOT(updateVisualization()));
00067   colorProperty_->hide();
00068 
00069   useRainbowProperty_ = new rviz::BoolProperty(
00070       "Use Rainbow", true,
00071       "Whether to use a rainbow of colors or to interpolate between two colors.", this,
00072       SLOT(updateUseRainbow()));
00073   
00074   invertRainbowProperty_ = new rviz::BoolProperty(
00075       "Invert Rainbow", false,
00076       "Whether to invert the rainbow colors.", this,
00077       SLOT(updateVisualization()));
00078 
00079   minColorProperty_ = new rviz::ColorProperty(
00080       "Min Color", QColor(0, 0, 0), "Color to assign to cells with the minimum intensity.  "
00081       "Actual color is interpolated between this and Max Color.",
00082       this, SLOT(updateVisualization()));
00083   minColorProperty_->hide();
00084 
00085   maxColorProperty_ = new rviz::ColorProperty(
00086       "Max Color", QColor(255, 255, 255), "Color to assign to cells with the maximum intensity.  "
00087       "Actual color is interpolated between Min Color and this.",
00088       this, SLOT(updateVisualization()));
00089   maxColorProperty_->hide();
00090 
00091   autocomputeIntensityBoundsProperty_ = new BoolProperty(
00092       "Autocompute Intensity Bounds", true,
00093       "Whether to automatically compute the intensity min/max values.", this,
00094       SLOT(updateAutocomputeIntensityBounds()));
00095 
00096   minIntensityProperty_ = new rviz::FloatProperty(
00097       "Min Intensity", 0.0,
00098       "Minimum possible intensity value, used to interpolate from Min Color to Max Color.", this,
00099       SLOT(updateVisualization()));
00100   minIntensityProperty_->hide();
00101 
00102   maxIntensityProperty_ = new rviz::FloatProperty(
00103       "Max Intensity", 10.0,
00104       "Maximum possible intensity value, used to interpolate from Min Color to Max Color.", this,
00105       SLOT(updateVisualization()));
00106   maxIntensityProperty_->hide();
00107 
00108   historyLengthProperty_->setMin(1);
00109   historyLengthProperty_->setMax(100);
00110 }
00111 
00112 GridMapDisplay::~GridMapDisplay()
00113 {
00114 }
00115 
00116 void GridMapDisplay::onInitialize()
00117 {
00118   MFDClass::onInitialize();      //  "MFDClass" = typedef of "MessageFilterDisplay<message type>"
00119   updateHistoryLength();
00120 }
00121 
00122 void GridMapDisplay::reset()
00123 {
00124   MFDClass::reset();
00125   visuals_.clear();
00126 }
00127 
00128 void GridMapDisplay::updateHistoryLength()
00129 {
00130   visuals_.rset_capacity(historyLengthProperty_->getInt());
00131 }
00132 
00133 void GridMapDisplay::updateHeightMode()
00134 {
00135   updateVisualization();
00136   heightTransformerProperty_->setHidden(heightModeProperty_->getOptionInt() == 1);
00137 }
00138 
00139 void GridMapDisplay::updateColorMode()
00140 {
00141   updateVisualization();
00142   
00143   bool intensityColor = colorModeProperty_->getOptionInt() == 0;
00144   bool flatColor = colorModeProperty_->getOptionInt() == 2;
00145   bool none = colorModeProperty_->getOptionInt() == 3;
00146   colorProperty_->setHidden(!flatColor);
00147   colorTransformerProperty_->setHidden(flatColor || none);
00148   useRainbowProperty_->setHidden(!intensityColor);
00149   invertRainbowProperty_->setHidden(!intensityColor);
00150   autocomputeIntensityBoundsProperty_->setHidden(!intensityColor);
00151   bool useRainbow = useRainbowProperty_->getBool();
00152   minColorProperty_->setHidden(!intensityColor || useRainbow);
00153   maxColorProperty_->setHidden(!intensityColor || useRainbow);
00154   bool autocomputeIntensity = autocomputeIntensityBoundsProperty_->getBool();
00155   minIntensityProperty_->setHidden(!intensityColor || autocomputeIntensity);
00156   minIntensityProperty_->setHidden(!intensityColor || autocomputeIntensity);
00157 }
00158 
00159 void GridMapDisplay::updateUseRainbow()
00160 {
00161   updateVisualization();
00162   bool useRainbow = useRainbowProperty_->getBool();
00163   minColorProperty_->setHidden(useRainbow);
00164   maxColorProperty_->setHidden(useRainbow);
00165   invertRainbowProperty_->setHidden(!useRainbow);
00166 }
00167 
00168 void GridMapDisplay::updateAutocomputeIntensityBounds()
00169 {
00170   updateVisualization();
00171   minIntensityProperty_->setHidden(autocomputeIntensityBoundsProperty_->getBool());
00172   maxIntensityProperty_->setHidden(autocomputeIntensityBoundsProperty_->getBool());
00173 }
00174 
00175 void GridMapDisplay::updateVisualization()
00176 {
00177   float alpha = alphaProperty_->getFloat();
00178   bool showGridLines = showGridLinesProperty_->getBool();
00179   bool flatTerrain = heightModeProperty_->getOptionInt() == 1;
00180   std::string heightLayer = heightTransformerProperty_->getStdString();
00181   bool mapLayerColor = colorModeProperty_->getOptionInt() == 1;
00182   bool flatColor = colorModeProperty_->getOptionInt() == 2;
00183   bool noColor = colorModeProperty_->getOptionInt() == 3;
00184   Ogre::ColourValue meshColor = colorProperty_->getOgreColor();
00185   std::string colorLayer = colorTransformerProperty_->getStdString();
00186   bool useRainbow = useRainbowProperty_->getBool();
00187   bool invertRainbow = invertRainbowProperty_->getBool();
00188   Ogre::ColourValue minColor = minColorProperty_->getOgreColor();
00189   Ogre::ColourValue maxColor = maxColorProperty_->getOgreColor();
00190   bool autocomputeIntensity = autocomputeIntensityBoundsProperty_->getBool();
00191   float minIntensity = minIntensityProperty_->getFloat();
00192   float maxIntensity = maxIntensityProperty_->getFloat();
00193 
00194   for (size_t i = 0; i < visuals_.size(); i++) {
00195     visuals_[i]->computeVisualization(alpha, showGridLines, flatTerrain, heightLayer, flatColor, noColor, meshColor,
00196                                       mapLayerColor, colorLayer, useRainbow, invertRainbow, minColor, maxColor,
00197                                       autocomputeIntensity, minIntensity, maxIntensity);
00198   }
00199 }
00200 
00201 void GridMapDisplay::processMessage(const grid_map_msgs::GridMap::ConstPtr& msg)
00202 {
00203   // Check if transform between the message's frame and the fixed frame exists.
00204   Ogre::Quaternion orientation;
00205   Ogre::Vector3 position;
00206   if (!context_->getFrameManager()->getTransform(msg->info.header.frame_id, msg->info.header.stamp,
00207                                                  position, orientation)) {
00208     ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->info.header.frame_id.c_str(),
00209               qPrintable(fixed_frame_));
00210     return;
00211   }
00212 
00213   boost::shared_ptr<GridMapVisual> visual;
00214   if (visuals_.full()) {
00215     visual = visuals_.front();
00216   } else {
00217     visual.reset(new GridMapVisual(context_->getSceneManager(), scene_node_));
00218   }
00219 
00220   visual->setMessage(msg);
00221   visual->setFramePosition(position);
00222   visual->setFrameOrientation(orientation);
00223 
00224   visual->computeVisualization(alphaProperty_->getFloat(), showGridLinesProperty_->getBool(),
00225                                heightModeProperty_->getOptionInt() == 1, heightTransformerProperty_->getStdString(),
00226                                colorModeProperty_->getOptionInt() == 2, colorModeProperty_->getOptionInt() == 3,
00227                                colorProperty_->getOgreColor(), colorModeProperty_->getOptionInt() == 1,
00228                                colorTransformerProperty_->getStdString(), useRainbowProperty_->getBool(),
00229                                invertRainbowProperty_->getBool(), minColorProperty_->getOgreColor(),
00230                                maxColorProperty_->getOgreColor(), autocomputeIntensityBoundsProperty_->getBool(),
00231                                minIntensityProperty_->getFloat(), maxIntensityProperty_->getFloat());
00232 
00233   std::vector<std::string> layer_names = visual->getLayerNames();
00234   heightTransformerProperty_->clearOptions();
00235   colorTransformerProperty_->clearOptions();
00236   for (size_t i = 0; i < layer_names.size(); i++) {
00237     heightTransformerProperty_->addOptionStd(layer_names[i]);
00238     colorTransformerProperty_->addOptionStd(layer_names[i]);
00239   }
00240 
00241   visuals_.push_back(visual);
00242 }
00243 
00244 }  // end namespace grid_map_rviz_plugin
00245 
00246 #include <pluginlib/class_list_macros.h>
00247 PLUGINLIB_EXPORT_CLASS(grid_map_rviz_plugin::GridMapDisplay, rviz::Display)


grid_map_rviz_plugin
Author(s): Philipp Krüsi, Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:33