00001
00002
00003
00004
00005
00006
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();
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
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 }
00245
00246 #include <pluginlib/class_list_macros.h>
00247 PLUGINLIB_EXPORT_CLASS(grid_map_rviz_plugin::GridMapDisplay, rviz::Display)