00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
00127 for( int i = 0; i <= 100; i++ )
00128 {
00129 unsigned char v = 255 - (255 * i) / 100;
00130 *palette_ptr++ = v;
00131 *palette_ptr++ = v;
00132 *palette_ptr++ = v;
00133 *palette_ptr++ = 255;
00134 }
00135
00136 for( int i = 101; i <= 127; i++ )
00137 {
00138 *palette_ptr++ = 0;
00139 *palette_ptr++ = 255;
00140 *palette_ptr++ = 0;
00141 *palette_ptr++ = 255;
00142 }
00143
00144 for( int i = 128; i <= 254; i++ )
00145 {
00146 *palette_ptr++ = 255;
00147 *palette_ptr++ = (255*(i-128))/(254-128);
00148 *palette_ptr++ = 0;
00149 *palette_ptr++ = 255;
00150 }
00151
00152 *palette_ptr++ = 0x70;
00153 *palette_ptr++ = 0x89;
00154 *palette_ptr++ = 0x86;
00155 *palette_ptr++ = 255;
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
00166 *palette_ptr++ = 0;
00167 *palette_ptr++ = 0;
00168 *palette_ptr++ = 0;
00169 *palette_ptr++ = 0;
00170
00171
00172 for( int i = 1; i <= 98; i++ )
00173 {
00174 unsigned char v = (255 * i) / 100;
00175 *palette_ptr++ = v;
00176 *palette_ptr++ = 0;
00177 *palette_ptr++ = 255 - v;
00178 *palette_ptr++ = 255;
00179 }
00180
00181 *palette_ptr++ = 0;
00182 *palette_ptr++ = 255;
00183 *palette_ptr++ = 255;
00184 *palette_ptr++ = 255;
00185
00186 *palette_ptr++ = 255;
00187 *palette_ptr++ = 255;
00188 *palette_ptr++ = 0;
00189 *palette_ptr++ = 255;
00190
00191 for( int i = 101; i <= 127; i++ )
00192 {
00193 *palette_ptr++ = 0;
00194 *palette_ptr++ = 255;
00195 *palette_ptr++ = 0;
00196 *palette_ptr++ = 255;
00197 }
00198
00199 for( int i = 128; i <= 254; i++ )
00200 {
00201 *palette_ptr++ = 255;
00202 *palette_ptr++ = (255*(i-128))/(254-128);
00203 *palette_ptr++ = 0;
00204 *palette_ptr++ = 255;
00205 }
00206
00207 *palette_ptr++ = 0x70;
00208 *palette_ptr++ = 0x89;
00209 *palette_ptr++ = 0x86;
00210 *palette_ptr++ = 255;
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
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
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
00257 {
00258
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
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
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
00275 {
00276
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
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
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
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
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
00458 Q_EMIT mapUpdated();
00459 loaded_ = true;
00460 }
00461
00462
00463 void MapDisplay::incomingUpdate(const map_msgs::OccupancyGridUpdate::ConstPtr& update)
00464 {
00465
00466 if( !loaded_ )
00467 {
00468 return;
00469 }
00470
00471
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
00482 for( size_t y = 0; y < update->height; y++ )
00483 {
00484 memcpy( ¤t_map_.data[ (update->y + y) * current_map_.info.width + update->x ],
00485 &update->data[ y * update->width ],
00486 update->width );
00487 }
00488
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
00554 if( current_map_.data.size() < pixels_size )
00555 {
00556 num_pixels_to_copy = current_map_.data.size();
00557 }
00558 }
00559
00560 memcpy( pixels, ¤t_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
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
00701 updateTopic();
00702 }
00703
00704 void MapDisplay::setTopic( const QString &topic, const QString &datatype )
00705 {
00706 topic_property_->setString( topic );
00707 }
00708
00709 }
00710
00711 #include <pluginlib/class_list_macros.h>
00712 PLUGINLIB_EXPORT_CLASS( rviz::MapDisplay, rviz::Display )