map_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <boost/bind.hpp>
00031 
00032 #include <OgreManualObject.h>
00033 #include <OgreMaterialManager.h>
00034 #include <OgreSceneManager.h>
00035 #include <OgreSceneNode.h>
00036 #include <OgreTextureManager.h>
00037 #include <OgreTechnique.h>
00038 #include <OgreSharedPtr.h>
00039 
00040 #include <ros/ros.h>
00041 
00042 #include <tf/transform_listener.h>
00043 
00044 #include "rviz/frame_manager.h"
00045 #include "rviz/ogre_helpers/custom_parameter_indices.h"
00046 #include "rviz/ogre_helpers/grid.h"
00047 #include "rviz/properties/enum_property.h"
00048 #include "rviz/properties/float_property.h"
00049 #include "rviz/properties/int_property.h"
00050 #include "rviz/properties/property.h"
00051 #include "rviz/properties/quaternion_property.h"
00052 #include "rviz/properties/ros_topic_property.h"
00053 #include "rviz/properties/vector_property.h"
00054 #include "rviz/validate_floats.h"
00055 #include "rviz/display_context.h"
00056 
00057 #include "map_display.h"
00058 
00059 namespace rviz
00060 {
00061 
00062 MapDisplay::MapDisplay()
00063   : Display()
00064   , manual_object_( NULL )
00065   , loaded_( false )
00066   , resolution_( 0.0f )
00067   , width_( 0 )
00068   , height_( 0 )
00069 {
00070   connect(this, SIGNAL( mapUpdated() ), this, SLOT( showMap() ));
00071   topic_property_ = new RosTopicProperty( "Topic", "",
00072                                           QString::fromStdString( ros::message_traits::datatype<nav_msgs::OccupancyGrid>() ),
00073                                           "nav_msgs::OccupancyGrid topic to subscribe to.",
00074                                           this, SLOT( updateTopic() ));
00075 
00076   alpha_property_ = new FloatProperty( "Alpha", 0.7,
00077                                        "Amount of transparency to apply to the map.",
00078                                        this, SLOT( updateAlpha() ));
00079   alpha_property_->setMin( 0 );
00080   alpha_property_->setMax( 1 );
00081 
00082   color_scheme_property_ = new EnumProperty( "Color Scheme", "map", "How to color the occupancy values.",
00083                                              this, SLOT( updatePalette() ));
00084   // Option values here must correspond to indices in palette_textures_ array in onInitialize() below.
00085   color_scheme_property_->addOption( "map", 0 );
00086   color_scheme_property_->addOption( "costmap", 1 );
00087 
00088   draw_under_property_ = new Property( "Draw Behind", false,
00089                                        "Rendering option, controls whether or not the map is always"
00090                                        " drawn behind everything else.",
00091                                        this, SLOT( updateDrawUnder() ));
00092 
00093   resolution_property_ = new FloatProperty( "Resolution", 0,
00094                                             "Resolution of the map. (not editable)", this );
00095   resolution_property_->setReadOnly( true );
00096 
00097   width_property_ = new IntProperty( "Width", 0,
00098                                      "Width of the map, in meters. (not editable)", this );
00099   width_property_->setReadOnly( true );
00100   
00101   height_property_ = new IntProperty( "Height", 0,
00102                                       "Height of the map, in meters. (not editable)", this );
00103   height_property_->setReadOnly( true );
00104 
00105   position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO,
00106                                            "Position of the bottom left corner of the map, in meters. (not editable)",
00107                                            this );
00108   position_property_->setReadOnly( true );
00109 
00110   orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY,
00111                                                   "Orientation of the map. (not editable)",
00112                                                   this );
00113   orientation_property_->setReadOnly( true );
00114 }
00115 
00116 MapDisplay::~MapDisplay()
00117 {
00118   unsubscribe();
00119   clear();
00120 }
00121 
00122 unsigned char* makeMapPalette()
00123 {
00124   unsigned char* palette = new unsigned char[256*4];
00125   unsigned char* palette_ptr = palette;
00126   // Standard gray map palette values
00127   for( int i = 0; i <= 100; i++ )
00128   {
00129     unsigned char v = 255 - (255 * i) / 100;
00130     *palette_ptr++ = v; // red
00131     *palette_ptr++ = v; // green
00132     *palette_ptr++ = v; // blue
00133     *palette_ptr++ = 255; // alpha
00134   }
00135   // illegal positive values in green
00136   for( int i = 101; i <= 127; i++ )
00137   {
00138     *palette_ptr++ = 0; // red
00139     *palette_ptr++ = 255; // green
00140     *palette_ptr++ = 0; // blue
00141     *palette_ptr++ = 255; // alpha
00142   }
00143   // illegal negative (char) values in shades of red/yellow
00144   for( int i = 128; i <= 254; i++ )
00145   {
00146     *palette_ptr++ = 255; // red
00147     *palette_ptr++ = (255*(i-128))/(254-128); // green
00148     *palette_ptr++ = 0; // blue
00149     *palette_ptr++ = 255; // alpha
00150   }
00151   // legal -1 value is tasteful blueish greenish grayish color
00152   *palette_ptr++ = 0x70; // red
00153   *palette_ptr++ = 0x89; // green
00154   *palette_ptr++ = 0x86; // blue
00155   *palette_ptr++ = 255; // alpha
00156 
00157   return palette;
00158 }
00159 
00160 unsigned char* makeCostmapPalette()
00161 {
00162   unsigned char* palette = new unsigned char[256*4];
00163   unsigned char* palette_ptr = palette;
00164 
00165   // zero values have alpha=0
00166   *palette_ptr++ = 0; // red
00167   *palette_ptr++ = 0; // green
00168   *palette_ptr++ = 0; // blue
00169   *palette_ptr++ = 0; // alpha
00170 
00171   // Blue to red spectrum for most normal cost values
00172   for( int i = 1; i <= 98; i++ )
00173   {
00174     unsigned char v = (255 * i) / 100;
00175     *palette_ptr++ = v; // red
00176     *palette_ptr++ = 0; // green
00177     *palette_ptr++ = 255 - v; // blue
00178     *palette_ptr++ = 255; // alpha
00179   }
00180   // inscribed obstacle values (99) in cyan
00181   *palette_ptr++ = 0; // red
00182   *palette_ptr++ = 255; // green
00183   *palette_ptr++ = 255; // blue
00184   *palette_ptr++ = 255; // alpha
00185   // lethal obstacle values (100) in purple
00186   *palette_ptr++ = 255; // red
00187   *palette_ptr++ = 255; // green
00188   *palette_ptr++ = 0; // blue
00189   *palette_ptr++ = 255; // alpha
00190   // illegal positive values in green
00191   for( int i = 101; i <= 127; i++ )
00192   {
00193     *palette_ptr++ = 0; // red
00194     *palette_ptr++ = 255; // green
00195     *palette_ptr++ = 0; // blue
00196     *palette_ptr++ = 255; // alpha
00197   }
00198   // illegal negative (char) values in shades of red/yellow
00199   for( int i = 128; i <= 254; i++ )
00200   {
00201     *palette_ptr++ = 255; // red
00202     *palette_ptr++ = (255*(i-128))/(254-128); // green
00203     *palette_ptr++ = 0; // blue
00204     *palette_ptr++ = 255; // alpha
00205   }
00206   // legal -1 value is tasteful blueish greenish grayish color
00207   *palette_ptr++ = 0x70; // red
00208   *palette_ptr++ = 0x89; // green
00209   *palette_ptr++ = 0x86; // blue
00210   *palette_ptr++ = 255; // alpha
00211 
00212   return palette;
00213 }
00214 
00215 Ogre::TexturePtr makePaletteTexture( unsigned char *palette_bytes )
00216 {
00217   Ogre::DataStreamPtr palette_stream;
00218   palette_stream.bind( new Ogre::MemoryDataStream( palette_bytes, 256*4 ));
00219 
00220   static int palette_tex_count = 0;
00221   std::stringstream ss;
00222   ss << "MapPaletteTexture" << palette_tex_count++;
00223   return Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00224                                                            palette_stream, 256, 1, Ogre::PF_BYTE_RGBA, Ogre::TEX_TYPE_1D, 0 );
00225 }
00226 
00227 void MapDisplay::onInitialize()
00228 {
00229   // Order of palette textures here must match option indices for color_scheme_property_ above.
00230   palette_textures_.push_back( makePaletteTexture( makeMapPalette() ));
00231   color_scheme_transparency_.push_back( false );
00232   palette_textures_.push_back( makePaletteTexture( makeCostmapPalette() ));
00233   color_scheme_transparency_.push_back( true );
00234 
00235   // Set up map material
00236   static int material_count = 0;
00237   std::stringstream ss;
00238   ss << "MapMaterial" << material_count++;
00239   material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/Indexed8BitImage");
00240   material_ = material_->clone( ss.str() );
00241 
00242   material_->setReceiveShadows(false);
00243   material_->getTechnique(0)->setLightingEnabled(false);
00244   material_->setDepthBias( -16.0f, 0.0f );
00245   material_->setCullingMode( Ogre::CULL_NONE );
00246   material_->setDepthWriteEnabled(false);
00247 
00248   static int map_count = 0;
00249   std::stringstream ss2;
00250   ss2 << "MapObject" << map_count++;
00251   manual_object_ = scene_manager_->createManualObject( ss2.str() );
00252   scene_node_->attachObject( manual_object_ );
00253 
00254   manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
00255   {
00256     // First triangle
00257     {
00258       // Bottom left
00259       manual_object_->position( 0.0f, 0.0f, 0.0f );
00260       manual_object_->textureCoord(0.0f, 0.0f);
00261       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00262 
00263       // Top right
00264       manual_object_->position( 1.0f, 1.0f, 0.0f );
00265       manual_object_->textureCoord(1.0f, 1.0f);
00266       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00267 
00268       // Top left
00269       manual_object_->position( 0.0f, 1.0f, 0.0f );
00270       manual_object_->textureCoord(0.0f, 1.0f);
00271       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00272     }
00273 
00274     // Second triangle
00275     {
00276       // Bottom left
00277       manual_object_->position( 0.0f, 0.0f, 0.0f );
00278       manual_object_->textureCoord(0.0f, 0.0f);
00279       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00280 
00281       // Bottom right
00282       manual_object_->position( 1.0f, 0.0f, 0.0f );
00283       manual_object_->textureCoord(1.0f, 0.0f);
00284       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00285 
00286       // Top right
00287       manual_object_->position( 1.0f, 1.0f, 0.0f );
00288       manual_object_->textureCoord(1.0f, 1.0f);
00289       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00290     }
00291   }
00292   manual_object_->end();
00293 
00294   if( draw_under_property_->getValue().toBool() )
00295   {
00296     manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4);
00297   }
00298 
00299   // don't show map until the plugin is actually enabled
00300   manual_object_->setVisible( false );
00301 
00302   updateAlpha();
00303 }
00304 
00305 void MapDisplay::onEnable()
00306 {
00307   subscribe();
00308 }
00309 
00310 void MapDisplay::onDisable()
00311 {
00312   unsubscribe();
00313   clear();
00314 }
00315 
00316 void MapDisplay::subscribe()
00317 {
00318   if ( !isEnabled() )
00319   {
00320     return;
00321   }
00322 
00323   if( !topic_property_->getTopic().isEmpty() )
00324   {
00325     try
00326     {
00327       map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &MapDisplay::incomingMap, this );
00328       setStatus( StatusProperty::Ok, "Topic", "OK" );
00329     }
00330     catch( ros::Exception& e )
00331     {
00332       setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00333     }
00334 
00335     try
00336     {
00337       update_sub_ = update_nh_.subscribe( topic_property_->getTopicStd() + "_updates", 1, &MapDisplay::incomingUpdate, this );
00338       setStatus( StatusProperty::Ok, "Update Topic", "OK" );
00339     }
00340     catch( ros::Exception& e )
00341     {
00342       setStatus( StatusProperty::Error, "Update Topic", QString( "Error subscribing: " ) + e.what() );
00343     }
00344   }
00345 }
00346 
00347 void MapDisplay::unsubscribe()
00348 {
00349   map_sub_.shutdown();
00350   update_sub_.shutdown();
00351 }
00352 
00353 // helper class to set alpha parameter on all renderables.
00354 class AlphaSetter: public Ogre::Renderable::Visitor
00355 {
00356 public:
00357   AlphaSetter( float alpha )
00358   : alpha_vec_( alpha, alpha, alpha, alpha )
00359   {}
00360 
00361   void visit( Ogre::Renderable *rend, ushort lodIndex, bool isDebug, Ogre::Any *pAny=0)
00362   {
00363     rend->setCustomParameter( ALPHA_PARAMETER, alpha_vec_ );
00364   }
00365 private:
00366   Ogre::Vector4 alpha_vec_;
00367 };
00368 
00369 void MapDisplay::updateAlpha()
00370 {
00371   float alpha = alpha_property_->getFloat();
00372 
00373   Ogre::Pass* pass = material_->getTechnique( 0 )->getPass( 0 );
00374   Ogre::TextureUnitState* tex_unit = NULL;
00375 
00376   if( alpha < 0.9998 || color_scheme_transparency_[ color_scheme_property_->getOptionInt() ])
00377   {
00378     material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00379     material_->setDepthWriteEnabled( false );
00380   }
00381   else
00382   {
00383     material_->setSceneBlending( Ogre::SBT_REPLACE );
00384     material_->setDepthWriteEnabled( !draw_under_property_->getValue().toBool() );
00385   }
00386 
00387   AlphaSetter alpha_setter( alpha );
00388   if( manual_object_ )
00389   {
00390     manual_object_->visitRenderables( &alpha_setter );
00391   }
00392 }
00393 
00394 void MapDisplay::updateDrawUnder()
00395 {
00396   bool draw_under = draw_under_property_->getValue().toBool();
00397 
00398   if( alpha_property_->getFloat() >= 0.9998 )
00399   {
00400     material_->setDepthWriteEnabled( !draw_under );
00401   }
00402 
00403   if( manual_object_ )
00404   {
00405     if( draw_under )
00406     {
00407       manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_4 );
00408     }
00409     else
00410     {
00411       manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN );
00412     }
00413   }
00414 }
00415 
00416 void MapDisplay::updateTopic()
00417 {
00418   unsubscribe();
00419   subscribe();
00420   clear();
00421 }
00422 
00423 void MapDisplay::clear()
00424 {
00425   setStatus( StatusProperty::Warn, "Message", "No map received" );
00426 
00427   if( !loaded_ )
00428   {
00429     return;
00430   }
00431 
00432   if( manual_object_ )
00433   {
00434     manual_object_->setVisible( false );
00435   }
00436 
00437   if( !texture_.isNull() )
00438   {
00439     Ogre::TextureManager::getSingleton().remove( texture_->getName() );
00440     texture_.setNull();
00441   }
00442 
00443   loaded_ = false;
00444 }
00445 
00446 bool validateFloats(const nav_msgs::OccupancyGrid& msg)
00447 {
00448   bool valid = true;
00449   valid = valid && validateFloats( msg.info.resolution );
00450   valid = valid && validateFloats( msg.info.origin );
00451   return valid;
00452 }
00453 
00454 void MapDisplay::incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
00455 {
00456   current_map_ = *msg;
00457   // updated via signal in case ros spinner is in a different thread
00458   Q_EMIT mapUpdated();
00459   loaded_ = true;
00460 }
00461 
00462 
00463 void MapDisplay::incomingUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr& update)
00464 {
00465   // Only update the map if we have gotten a full one first.
00466   if( !loaded_ )
00467   {
00468     return;
00469   }
00470 
00471   // Reject updates which have any out-of-bounds data.
00472   if( update->x < 0 ||
00473       update->y < 0 ||
00474       current_map_.info.width < update->x + update->width ||
00475       current_map_.info.height < update->y + update->height )
00476   {
00477     setStatus( StatusProperty::Error, "Update", "Update area outside of original map area." );
00478     return;
00479   }
00480 
00481   // Copy the incoming data into current_map_'s data.
00482   for( size_t y = 0; y < update->height; y++ )
00483   {
00484     memcpy( &current_map_.data[ (update->y + y) * current_map_.info.width + update->x ],
00485             &update->data[ y * update->width ],
00486             update->width );
00487   }
00488   // updated via signal in case ros spinner is in a different thread
00489   Q_EMIT mapUpdated();
00490 }
00491 
00492 void MapDisplay::showMap()
00493 {
00494   if (current_map_.data.empty())
00495   {
00496     return;
00497   }
00498 
00499   if( !validateFloats( current_map_ ))
00500   {
00501     setStatus( StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or infs)" );
00502     return;
00503   }
00504 
00505   if( current_map_.info.width * current_map_.info.height == 0 )
00506   {
00507     std::stringstream ss;
00508     ss << "Map is zero-sized (" << current_map_.info.width << "x" << current_map_.info.height << ")";
00509     setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
00510     return;
00511   }
00512 
00513   setStatus( StatusProperty::Ok, "Message", "Map received" );
00514 
00515   ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n",
00516              current_map_.info.width,
00517              current_map_.info.height,
00518              current_map_.info.resolution );
00519 
00520   float resolution = current_map_.info.resolution;
00521 
00522   int width = current_map_.info.width;
00523   int height = current_map_.info.height;
00524 
00525 
00526   Ogre::Vector3 position( current_map_.info.origin.position.x,
00527                           current_map_.info.origin.position.y,
00528                           current_map_.info.origin.position.z );
00529   Ogre::Quaternion orientation( current_map_.info.origin.orientation.w,
00530                                 current_map_.info.origin.orientation.x,
00531                                 current_map_.info.origin.orientation.y,
00532                                 current_map_.info.origin.orientation.z );
00533   frame_ = current_map_.header.frame_id;
00534   if (frame_.empty())
00535   {
00536     frame_ = "/map";
00537   }
00538 
00539   unsigned int pixels_size = width * height;
00540   unsigned char* pixels = new unsigned char[pixels_size];
00541   memset(pixels, 255, pixels_size);
00542 
00543   bool map_status_set = false;
00544   unsigned int num_pixels_to_copy = pixels_size;
00545   if( pixels_size != current_map_.data.size() )
00546   {
00547     std::stringstream ss;
00548     ss << "Data size doesn't match width*height: width = " << width
00549        << ", height = " << height << ", data size = " << current_map_.data.size();
00550     setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
00551     map_status_set = true;
00552 
00553     // Keep going, but don't read past the end of the data.
00554     if( current_map_.data.size() < pixels_size )
00555     {
00556       num_pixels_to_copy = current_map_.data.size();
00557     }
00558   }
00559 
00560   memcpy( pixels, &current_map_.data[0], num_pixels_to_copy );
00561 
00562   Ogre::DataStreamPtr pixel_stream;
00563   pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size ));
00564 
00565   if( !texture_.isNull() )
00566   {
00567     Ogre::TextureManager::getSingleton().remove( texture_->getName() );
00568     texture_.setNull();
00569   }
00570 
00571   static int tex_count = 0;
00572   std::stringstream ss;
00573   ss << "MapTexture" << tex_count++;
00574   try
00575   {
00576     texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00577                                                                  pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D,
00578                                                                  0);
00579 
00580     if( !map_status_set )
00581     {
00582       setStatus( StatusProperty::Ok, "Map", "Map OK" );
00583     }
00584   }
00585   catch(Ogre::RenderingAPIException&)
00586   {
00587     Ogre::Image image;
00588     pixel_stream->seek(0);
00589     float fwidth = width;
00590     float fheight = height;
00591     if( width > height )
00592     {
00593       float aspect = fheight / fwidth;
00594       fwidth = 2048;
00595       fheight = fwidth * aspect;
00596     }
00597     else
00598     {
00599       float aspect = fwidth / fheight;
00600       fheight = 2048;
00601       fwidth = fheight * aspect;
00602     }
00603 
00604     {
00605       std::stringstream ss;
00606       ss << "Map is larger than your graphics card supports.  Downsampled from [" << width << "x" << height << "] to [" << fwidth << "x" << fheight << "]";
00607       setStatus(StatusProperty::Ok, "Map", QString::fromStdString( ss.str() ));
00608     }
00609 
00610     ROS_WARN("Failed to create full-size map texture, likely because your graphics card does not support textures of size > 2048.  Downsampling to [%d x %d]...", (int)fwidth, (int)fheight);
00611     //ROS_INFO("Stream size [%d], width [%f], height [%f], w * h [%f]", pixel_stream->size(), width, height, width * height);
00612     image.loadRawData(pixel_stream, width, height, Ogre::PF_L8);
00613     image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST);
00614     ss << "Downsampled";
00615     texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
00616   }
00617 
00618   delete [] pixels;
00619 
00620   Ogre::Pass* pass = material_->getTechnique(0)->getPass(0);
00621   Ogre::TextureUnitState* tex_unit = NULL;
00622   if (pass->getNumTextureUnitStates() > 0)
00623   {
00624     tex_unit = pass->getTextureUnitState(0);
00625   }
00626   else
00627   {
00628     tex_unit = pass->createTextureUnitState();
00629   }
00630 
00631   tex_unit->setTextureName(texture_->getName());
00632   tex_unit->setTextureFiltering( Ogre::TFO_NONE );
00633 
00634   updatePalette();
00635 
00636   resolution_property_->setValue( resolution );
00637   width_property_->setValue( width );
00638   height_property_->setValue( height );
00639   position_property_->setVector( position );
00640   orientation_property_->setQuaternion( orientation );
00641 
00642   transformMap();
00643   manual_object_->setVisible( true );
00644   scene_node_->setScale( resolution * width, resolution * height, 1.0 );
00645 
00646   context_->queueRender();
00647 }
00648 
00649 void MapDisplay::updatePalette()
00650 {
00651   int palette_index = color_scheme_property_->getOptionInt();
00652 
00653   Ogre::Pass* pass = material_->getTechnique(0)->getPass(0);
00654   Ogre::TextureUnitState* palette_tex_unit = NULL;
00655   if( pass->getNumTextureUnitStates() > 1 )
00656   {
00657     palette_tex_unit = pass->getTextureUnitState( 1 );
00658   }
00659   else
00660   {
00661     palette_tex_unit = pass->createTextureUnitState();
00662   }
00663   palette_tex_unit->setTextureName( palette_textures_[ palette_index ]->getName() );
00664   palette_tex_unit->setTextureFiltering( Ogre::TFO_NONE );
00665 
00666   updateAlpha();
00667 }
00668 
00669 void MapDisplay::transformMap()
00670 {
00671   Ogre::Vector3 position;
00672   Ogre::Quaternion orientation;
00673   if (!context_->getFrameManager()->transform(frame_, ros::Time(), current_map_.info.origin, position, orientation))
00674   {
00675     ROS_DEBUG( "Error transforming map '%s' from frame '%s' to frame '%s'",
00676                qPrintable( getName() ), frame_.c_str(), qPrintable( fixed_frame_ ));
00677 
00678     setStatus( StatusProperty::Error, "Transform",
00679                "No transform from [" + QString::fromStdString( frame_ ) + "] to [" + fixed_frame_ + "]" );
00680   }
00681   else
00682   {
00683     setStatus(StatusProperty::Ok, "Transform", "Transform OK");
00684   }
00685 
00686   scene_node_->setPosition( position );
00687   scene_node_->setOrientation( orientation );
00688 }
00689 
00690 void MapDisplay::fixedFrameChanged()
00691 {
00692   transformMap();
00693 }
00694 
00695 void MapDisplay::reset()
00696 {
00697   Display::reset();
00698 
00699   clear();
00700   // Force resubscription so that the map will be re-sent
00701   updateTopic();
00702 }
00703 
00704 void MapDisplay::setTopic( const QString &topic, const QString &datatype )
00705 {
00706   topic_property_->setString( topic );
00707 }
00708 
00709 } // namespace rviz
00710 
00711 #include <pluginlib/class_list_macros.h>
00712 PLUGINLIB_EXPORT_CLASS( rviz::MapDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27