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   color_scheme_property_->addOption( "raw", 2 );
00088 
00089   draw_under_property_ = new Property( "Draw Behind", false,
00090                                        "Rendering option, controls whether or not the map is always"
00091                                        " drawn behind everything else.",
00092                                        this, SLOT( updateDrawUnder() ));
00093 
00094   resolution_property_ = new FloatProperty( "Resolution", 0,
00095                                             "Resolution of the map. (not editable)", this );
00096   resolution_property_->setReadOnly( true );
00097 
00098   width_property_ = new IntProperty( "Width", 0,
00099                                      "Width of the map, in meters. (not editable)", this );
00100   width_property_->setReadOnly( true );
00101   
00102   height_property_ = new IntProperty( "Height", 0,
00103                                       "Height of the map, in meters. (not editable)", this );
00104   height_property_->setReadOnly( true );
00105 
00106   position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO,
00107                                            "Position of the bottom left corner of the map, in meters. (not editable)",
00108                                            this );
00109   position_property_->setReadOnly( true );
00110 
00111   orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY,
00112                                                   "Orientation of the map. (not editable)",
00113                                                   this );
00114   orientation_property_->setReadOnly( true );
00115 
00116   unreliable_property_ = new BoolProperty( "Unreliable", false,
00117                                            "Prefer UDP topic transport",
00118                                            this,
00119                                            SLOT( updateTopic() ));
00120 }
00121 
00122 MapDisplay::~MapDisplay()
00123 {
00124   unsubscribe();
00125   clear();
00126 }
00127 
00128 unsigned char* makeMapPalette()
00129 {
00130   unsigned char* palette = new unsigned char[256*4];
00131   unsigned char* palette_ptr = palette;
00132   // Standard gray map palette values
00133   for( int i = 0; i <= 100; i++ )
00134   {
00135     unsigned char v = 255 - (255 * i) / 100;
00136     *palette_ptr++ = v; // red
00137     *palette_ptr++ = v; // green
00138     *palette_ptr++ = v; // blue
00139     *palette_ptr++ = 255; // alpha
00140   }
00141   // illegal positive values in green
00142   for( int i = 101; i <= 127; i++ )
00143   {
00144     *palette_ptr++ = 0; // red
00145     *palette_ptr++ = 255; // green
00146     *palette_ptr++ = 0; // blue
00147     *palette_ptr++ = 255; // alpha
00148   }
00149   // illegal negative (char) values in shades of red/yellow
00150   for( int i = 128; i <= 254; i++ )
00151   {
00152     *palette_ptr++ = 255; // red
00153     *palette_ptr++ = (255*(i-128))/(254-128); // green
00154     *palette_ptr++ = 0; // blue
00155     *palette_ptr++ = 255; // alpha
00156   }
00157   // legal -1 value is tasteful blueish greenish grayish color
00158   *palette_ptr++ = 0x70; // red
00159   *palette_ptr++ = 0x89; // green
00160   *palette_ptr++ = 0x86; // blue
00161   *palette_ptr++ = 255; // alpha
00162 
00163   return palette;
00164 }
00165 
00166 unsigned char* makeCostmapPalette()
00167 {
00168   unsigned char* palette = new unsigned char[256*4];
00169   unsigned char* palette_ptr = palette;
00170 
00171   // zero values have alpha=0
00172   *palette_ptr++ = 0; // red
00173   *palette_ptr++ = 0; // green
00174   *palette_ptr++ = 0; // blue
00175   *palette_ptr++ = 0; // alpha
00176 
00177   // Blue to red spectrum for most normal cost values
00178   for( int i = 1; i <= 98; i++ )
00179   {
00180     unsigned char v = (255 * i) / 100;
00181     *palette_ptr++ = v; // red
00182     *palette_ptr++ = 0; // green
00183     *palette_ptr++ = 255 - v; // blue
00184     *palette_ptr++ = 255; // alpha
00185   }
00186   // inscribed obstacle values (99) in cyan
00187   *palette_ptr++ = 0; // red
00188   *palette_ptr++ = 255; // green
00189   *palette_ptr++ = 255; // blue
00190   *palette_ptr++ = 255; // alpha
00191   // lethal obstacle values (100) in yellow
00192   *palette_ptr++ = 255; // red
00193   *palette_ptr++ = 255; // green
00194   *palette_ptr++ = 0; // blue
00195   *palette_ptr++ = 255; // alpha
00196   // illegal positive values in green
00197   for( int i = 101; i <= 127; i++ )
00198   {
00199     *palette_ptr++ = 0; // red
00200     *palette_ptr++ = 255; // green
00201     *palette_ptr++ = 0; // blue
00202     *palette_ptr++ = 255; // alpha
00203   }
00204   // illegal negative (char) values in shades of red/yellow
00205   for( int i = 128; i <= 254; i++ )
00206   {
00207     *palette_ptr++ = 255; // red
00208     *palette_ptr++ = (255*(i-128))/(254-128); // green
00209     *palette_ptr++ = 0; // blue
00210     *palette_ptr++ = 255; // alpha
00211   }
00212   // legal -1 value is tasteful blueish greenish grayish color
00213   *palette_ptr++ = 0x70; // red
00214   *palette_ptr++ = 0x89; // green
00215   *palette_ptr++ = 0x86; // blue
00216   *palette_ptr++ = 255; // alpha
00217 
00218   return palette;
00219 }
00220 
00221 unsigned char* makeRawPalette()
00222 {
00223   unsigned char* palette = new unsigned char[256*4];
00224   unsigned char* palette_ptr = palette;
00225   // Standard gray map palette values
00226   for( int i = 0; i < 256; i++ )
00227   {
00228     *palette_ptr++ = i; // red
00229     *palette_ptr++ = i; // green
00230     *palette_ptr++ = i; // blue
00231     *palette_ptr++ = 255; // alpha
00232   }
00233 
00234   return palette;
00235 }
00236 
00237 Ogre::TexturePtr makePaletteTexture( unsigned char *palette_bytes )
00238 {
00239   Ogre::DataStreamPtr palette_stream;
00240   palette_stream.bind( new Ogre::MemoryDataStream( palette_bytes, 256*4 ));
00241 
00242   static int palette_tex_count = 0;
00243   std::stringstream ss;
00244   ss << "MapPaletteTexture" << palette_tex_count++;
00245   return Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00246                                                            palette_stream, 256, 1, Ogre::PF_BYTE_RGBA, Ogre::TEX_TYPE_1D, 0 );
00247 }
00248 
00249 void MapDisplay::onInitialize()
00250 {
00251   // Order of palette textures here must match option indices for color_scheme_property_ above.
00252   palette_textures_.push_back( makePaletteTexture( makeMapPalette() ));
00253   color_scheme_transparency_.push_back( false );
00254   palette_textures_.push_back( makePaletteTexture( makeCostmapPalette() ));
00255   color_scheme_transparency_.push_back( true );
00256   palette_textures_.push_back( makePaletteTexture( makeRawPalette() ));
00257   color_scheme_transparency_.push_back( true );
00258 
00259   // Set up map material
00260   static int material_count = 0;
00261   std::stringstream ss;
00262   ss << "MapMaterial" << material_count++;
00263   material_ = Ogre::MaterialManager::getSingleton().getByName("rviz/Indexed8BitImage");
00264   material_ = material_->clone( ss.str() );
00265 
00266   material_->setReceiveShadows(false);
00267   material_->getTechnique(0)->setLightingEnabled(false);
00268   material_->setDepthBias( -16.0f, 0.0f );
00269   material_->setCullingMode( Ogre::CULL_NONE );
00270   material_->setDepthWriteEnabled(false);
00271 
00272   static int map_count = 0;
00273   std::stringstream ss2;
00274   ss2 << "MapObject" << map_count++;
00275   manual_object_ = scene_manager_->createManualObject( ss2.str() );
00276   scene_node_->attachObject( manual_object_ );
00277 
00278   manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
00279   {
00280     // First triangle
00281     {
00282       // Bottom left
00283       manual_object_->position( 0.0f, 0.0f, 0.0f );
00284       manual_object_->textureCoord(0.0f, 0.0f);
00285       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00286 
00287       // Top right
00288       manual_object_->position( 1.0f, 1.0f, 0.0f );
00289       manual_object_->textureCoord(1.0f, 1.0f);
00290       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00291 
00292       // Top left
00293       manual_object_->position( 0.0f, 1.0f, 0.0f );
00294       manual_object_->textureCoord(0.0f, 1.0f);
00295       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00296     }
00297 
00298     // Second triangle
00299     {
00300       // Bottom left
00301       manual_object_->position( 0.0f, 0.0f, 0.0f );
00302       manual_object_->textureCoord(0.0f, 0.0f);
00303       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00304 
00305       // Bottom right
00306       manual_object_->position( 1.0f, 0.0f, 0.0f );
00307       manual_object_->textureCoord(1.0f, 0.0f);
00308       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00309 
00310       // Top right
00311       manual_object_->position( 1.0f, 1.0f, 0.0f );
00312       manual_object_->textureCoord(1.0f, 1.0f);
00313       manual_object_->normal( 0.0f, 0.0f, 1.0f );
00314     }
00315   }
00316   manual_object_->end();
00317 
00318   if( draw_under_property_->getValue().toBool() )
00319   {
00320     manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4);
00321   }
00322 
00323   // don't show map until the plugin is actually enabled
00324   manual_object_->setVisible( false );
00325 
00326   updateAlpha();
00327 }
00328 
00329 void MapDisplay::onEnable()
00330 {
00331   subscribe();
00332 }
00333 
00334 void MapDisplay::onDisable()
00335 {
00336   unsubscribe();
00337   clear();
00338 }
00339 
00340 void MapDisplay::subscribe()
00341 {
00342   if ( !isEnabled() )
00343   {
00344     return;
00345   }
00346 
00347   if( !topic_property_->getTopic().isEmpty() )
00348   {
00349     try
00350     {
00351       if(unreliable_property_->getBool())
00352       {
00353         map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &MapDisplay::incomingMap, this,  ros::TransportHints().unreliable());
00354       }else{
00355         map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &MapDisplay::incomingMap, this, ros::TransportHints().reliable() );
00356       }
00357       setStatus( StatusProperty::Ok, "Topic", "OK" );
00358     }
00359     catch( ros::Exception& e )
00360     {
00361       setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00362     }
00363 
00364     try
00365     {
00366       update_sub_ = update_nh_.subscribe( topic_property_->getTopicStd() + "_updates", 1, &MapDisplay::incomingUpdate, this );
00367       setStatus( StatusProperty::Ok, "Update Topic", "OK" );
00368     }
00369     catch( ros::Exception& e )
00370     {
00371       setStatus( StatusProperty::Error, "Update Topic", QString( "Error subscribing: " ) + e.what() );
00372     }
00373   }
00374 }
00375 
00376 void MapDisplay::unsubscribe()
00377 {
00378   map_sub_.shutdown();
00379   update_sub_.shutdown();
00380 }
00381 
00382 // helper class to set alpha parameter on all renderables.
00383 class AlphaSetter: public Ogre::Renderable::Visitor
00384 {
00385 public:
00386   AlphaSetter( float alpha )
00387   : alpha_vec_( alpha, alpha, alpha, alpha )
00388   {}
00389 
00390   void visit( Ogre::Renderable *rend, ushort lodIndex, bool isDebug, Ogre::Any *pAny=0)
00391   {
00392     rend->setCustomParameter( ALPHA_PARAMETER, alpha_vec_ );
00393   }
00394 private:
00395   Ogre::Vector4 alpha_vec_;
00396 };
00397 
00398 void MapDisplay::updateAlpha()
00399 {
00400   float alpha = alpha_property_->getFloat();
00401 
00402   Ogre::Pass* pass = material_->getTechnique( 0 )->getPass( 0 );
00403   Ogre::TextureUnitState* tex_unit = NULL;
00404 
00405   if( alpha < 0.9998 || color_scheme_transparency_[ color_scheme_property_->getOptionInt() ])
00406   {
00407     material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00408     material_->setDepthWriteEnabled( false );
00409   }
00410   else
00411   {
00412     material_->setSceneBlending( Ogre::SBT_REPLACE );
00413     material_->setDepthWriteEnabled( !draw_under_property_->getValue().toBool() );
00414   }
00415 
00416   AlphaSetter alpha_setter( alpha );
00417   if( manual_object_ )
00418   {
00419     manual_object_->visitRenderables( &alpha_setter );
00420   }
00421 }
00422 
00423 void MapDisplay::updateDrawUnder()
00424 {
00425   bool draw_under = draw_under_property_->getValue().toBool();
00426 
00427   if( alpha_property_->getFloat() >= 0.9998 )
00428   {
00429     material_->setDepthWriteEnabled( !draw_under );
00430   }
00431 
00432   if( manual_object_ )
00433   {
00434     if( draw_under )
00435     {
00436       manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_4 );
00437     }
00438     else
00439     {
00440       manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN );
00441     }
00442   }
00443 }
00444 
00445 void MapDisplay::updateTopic()
00446 {
00447   unsubscribe();
00448   subscribe();
00449   clear();
00450 }
00451 
00452 void MapDisplay::clear()
00453 {
00454   setStatus( StatusProperty::Warn, "Message", "No map received" );
00455 
00456   if( !loaded_ )
00457   {
00458     return;
00459   }
00460 
00461   if( manual_object_ )
00462   {
00463     manual_object_->setVisible( false );
00464   }
00465 
00466   if( !texture_.isNull() )
00467   {
00468     Ogre::TextureManager::getSingleton().remove( texture_->getName() );
00469     texture_.setNull();
00470   }
00471 
00472   loaded_ = false;
00473 }
00474 
00475 bool validateFloats(const nav_msgs::OccupancyGrid& msg)
00476 {
00477   bool valid = true;
00478   valid = valid && validateFloats( msg.info.resolution );
00479   valid = valid && validateFloats( msg.info.origin );
00480   return valid;
00481 }
00482 
00483 void MapDisplay::incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
00484 {
00485   current_map_ = *msg;
00486   // updated via signal in case ros spinner is in a different thread
00487   Q_EMIT mapUpdated();
00488   loaded_ = true;
00489 }
00490 
00491 
00492 void MapDisplay::incomingUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr& update)
00493 {
00494   // Only update the map if we have gotten a full one first.
00495   if( !loaded_ )
00496   {
00497     return;
00498   }
00499 
00500   // Reject updates which have any out-of-bounds data.
00501   if( update->x < 0 ||
00502       update->y < 0 ||
00503       current_map_.info.width < update->x + update->width ||
00504       current_map_.info.height < update->y + update->height )
00505   {
00506     setStatus( StatusProperty::Error, "Update", "Update area outside of original map area." );
00507     return;
00508   }
00509 
00510   // Copy the incoming data into current_map_'s data.
00511   for( size_t y = 0; y < update->height; y++ )
00512   {
00513     memcpy( &current_map_.data[ (update->y + y) * current_map_.info.width + update->x ],
00514             &update->data[ y * update->width ],
00515             update->width );
00516   }
00517   // updated via signal in case ros spinner is in a different thread
00518   Q_EMIT mapUpdated();
00519 }
00520 
00521 void MapDisplay::showMap()
00522 {
00523   if (current_map_.data.empty())
00524   {
00525     return;
00526   }
00527 
00528   if( !validateFloats( current_map_ ))
00529   {
00530     setStatus( StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or infs)" );
00531     return;
00532   }
00533 
00534   if( current_map_.info.width * current_map_.info.height == 0 )
00535   {
00536     std::stringstream ss;
00537     ss << "Map is zero-sized (" << current_map_.info.width << "x" << current_map_.info.height << ")";
00538     setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
00539     return;
00540   }
00541 
00542   setStatus( StatusProperty::Ok, "Message", "Map received" );
00543 
00544   ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n",
00545              current_map_.info.width,
00546              current_map_.info.height,
00547              current_map_.info.resolution );
00548 
00549   float resolution = current_map_.info.resolution;
00550 
00551   int width = current_map_.info.width;
00552   int height = current_map_.info.height;
00553 
00554 
00555   Ogre::Vector3 position( current_map_.info.origin.position.x,
00556                           current_map_.info.origin.position.y,
00557                           current_map_.info.origin.position.z );
00558   Ogre::Quaternion orientation( current_map_.info.origin.orientation.w,
00559                                 current_map_.info.origin.orientation.x,
00560                                 current_map_.info.origin.orientation.y,
00561                                 current_map_.info.origin.orientation.z );
00562   frame_ = current_map_.header.frame_id;
00563   if (frame_.empty())
00564   {
00565     frame_ = "/map";
00566   }
00567 
00568   unsigned int pixels_size = width * height;
00569   unsigned char* pixels = new unsigned char[pixels_size];
00570   memset(pixels, 255, pixels_size);
00571 
00572   bool map_status_set = false;
00573   unsigned int num_pixels_to_copy = pixels_size;
00574   if( pixels_size != current_map_.data.size() )
00575   {
00576     std::stringstream ss;
00577     ss << "Data size doesn't match width*height: width = " << width
00578        << ", height = " << height << ", data size = " << current_map_.data.size();
00579     setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
00580     map_status_set = true;
00581 
00582     // Keep going, but don't read past the end of the data.
00583     if( current_map_.data.size() < pixels_size )
00584     {
00585       num_pixels_to_copy = current_map_.data.size();
00586     }
00587   }
00588 
00589   memcpy( pixels, &current_map_.data[0], num_pixels_to_copy );
00590 
00591   Ogre::DataStreamPtr pixel_stream;
00592   pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size ));
00593 
00594   if( !texture_.isNull() )
00595   {
00596     Ogre::TextureManager::getSingleton().remove( texture_->getName() );
00597     texture_.setNull();
00598   }
00599 
00600   static int tex_count = 0;
00601   std::stringstream ss;
00602   ss << "MapTexture" << tex_count++;
00603   try
00604   {
00605     texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00606                                                                  pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D,
00607                                                                  0);
00608 
00609     if( !map_status_set )
00610     {
00611       setStatus( StatusProperty::Ok, "Map", "Map OK" );
00612     }
00613   }
00614   catch(Ogre::RenderingAPIException&)
00615   {
00616     Ogre::Image image;
00617     pixel_stream->seek(0);
00618     float fwidth = width;
00619     float fheight = height;
00620     if( width > height )
00621     {
00622       float aspect = fheight / fwidth;
00623       fwidth = 2048;
00624       fheight = fwidth * aspect;
00625     }
00626     else
00627     {
00628       float aspect = fwidth / fheight;
00629       fheight = 2048;
00630       fwidth = fheight * aspect;
00631     }
00632 
00633     {
00634       std::stringstream ss;
00635       ss << "Map is larger than your graphics card supports.  Downsampled from [" << width << "x" << height << "] to [" << fwidth << "x" << fheight << "]";
00636       setStatus(StatusProperty::Warn, "Map", QString::fromStdString( ss.str() ));
00637     }
00638 
00639     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);
00640     //ROS_INFO("Stream size [%d], width [%f], height [%f], w * h [%f]", pixel_stream->size(), width, height, width * height);
00641     image.loadRawData(pixel_stream, width, height, Ogre::PF_L8);
00642     image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST);
00643     ss << "Downsampled";
00644     texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
00645   }
00646 
00647   delete [] pixels;
00648 
00649   Ogre::Pass* pass = material_->getTechnique(0)->getPass(0);
00650   Ogre::TextureUnitState* tex_unit = NULL;
00651   if (pass->getNumTextureUnitStates() > 0)
00652   {
00653     tex_unit = pass->getTextureUnitState(0);
00654   }
00655   else
00656   {
00657     tex_unit = pass->createTextureUnitState();
00658   }
00659 
00660   tex_unit->setTextureName(texture_->getName());
00661   tex_unit->setTextureFiltering( Ogre::TFO_NONE );
00662 
00663   updatePalette();
00664 
00665   resolution_property_->setValue( resolution );
00666   width_property_->setValue( width );
00667   height_property_->setValue( height );
00668   position_property_->setVector( position );
00669   orientation_property_->setQuaternion( orientation );
00670 
00671   transformMap();
00672   manual_object_->setVisible( true );
00673   scene_node_->setScale( resolution * width, resolution * height, 1.0 );
00674 
00675   context_->queueRender();
00676 }
00677 
00678 void MapDisplay::updatePalette()
00679 {
00680   int palette_index = color_scheme_property_->getOptionInt();
00681 
00682   Ogre::Pass* pass = material_->getTechnique(0)->getPass(0);
00683   Ogre::TextureUnitState* palette_tex_unit = NULL;
00684   if( pass->getNumTextureUnitStates() > 1 )
00685   {
00686     palette_tex_unit = pass->getTextureUnitState( 1 );
00687   }
00688   else
00689   {
00690     palette_tex_unit = pass->createTextureUnitState();
00691   }
00692   palette_tex_unit->setTextureName( palette_textures_[ palette_index ]->getName() );
00693   palette_tex_unit->setTextureFiltering( Ogre::TFO_NONE );
00694 
00695   updateAlpha();
00696 }
00697 
00698 void MapDisplay::transformMap()
00699 {
00700   if (!loaded_)
00701   {
00702     return;
00703   }
00704 
00705   Ogre::Vector3 position;
00706   Ogre::Quaternion orientation;
00707   if (!context_->getFrameManager()->transform(frame_, ros::Time(), current_map_.info.origin, position, orientation))
00708   {
00709     ROS_DEBUG( "Error transforming map '%s' from frame '%s' to frame '%s'",
00710                qPrintable( getName() ), frame_.c_str(), qPrintable( fixed_frame_ ));
00711 
00712     setStatus( StatusProperty::Error, "Transform",
00713                "No transform from [" + QString::fromStdString( frame_ ) + "] to [" + fixed_frame_ + "]" );
00714   }
00715   else
00716   {
00717     setStatus(StatusProperty::Ok, "Transform", "Transform OK");
00718   }
00719 
00720   scene_node_->setPosition( position );
00721   scene_node_->setOrientation( orientation );
00722 }
00723 
00724 void MapDisplay::fixedFrameChanged()
00725 {
00726   transformMap();
00727 }
00728 
00729 void MapDisplay::reset()
00730 {
00731   Display::reset();
00732 
00733   clear();
00734   // Force resubscription so that the map will be re-sent
00735   updateTopic();
00736 }
00737 
00738 void MapDisplay::setTopic( const QString &topic, const QString &datatype )
00739 {
00740   topic_property_->setString( topic );
00741 }
00742 
00743 void MapDisplay::update( float wall_dt, float ros_dt ) {
00744   transformMap();
00745 }
00746 
00747 } // namespace rviz
00748 
00749 #include <pluginlib/class_list_macros.h>
00750 PLUGINLIB_EXPORT_CLASS( rviz::MapDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:31