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 <OGRE/OgreManualObject.h>
00033 #include <OGRE/OgreMaterialManager.h>
00034 #include <OGRE/OgreSceneManager.h>
00035 #include <OGRE/OgreSceneNode.h>
00036 #include <OGRE/OgreTextureManager.h>
00037
00038 #include <ros/ros.h>
00039
00040 #include <tf/transform_listener.h>
00041
00042 #include "rviz/frame_manager.h"
00043 #include "rviz/ogre_helpers/grid.h"
00044 #include "rviz/properties/float_property.h"
00045 #include "rviz/properties/int_property.h"
00046 #include "rviz/properties/property.h"
00047 #include "rviz/properties/quaternion_property.h"
00048 #include "rviz/properties/ros_topic_property.h"
00049 #include "rviz/properties/vector_property.h"
00050 #include "rviz/validate_floats.h"
00051 #include "rviz/display_context.h"
00052
00053 #include "map_display.h"
00054
00055 namespace rviz
00056 {
00057
00058 MapDisplay::MapDisplay()
00059 : Display()
00060 , scene_node_( 0 )
00061 , manual_object_( NULL )
00062 , material_( 0 )
00063 , loaded_( false )
00064 , resolution_( 0.0f )
00065 , width_( 0 )
00066 , height_( 0 )
00067 , position_(Ogre::Vector3::ZERO)
00068 , orientation_(Ogre::Quaternion::IDENTITY)
00069 {
00070 topic_property_ = new RosTopicProperty( "Topic", "",
00071 QString::fromStdString( ros::message_traits::datatype<nav_msgs::OccupancyGrid>() ),
00072 "nav_msgs::OccupancyGrid topic to subscribe to.",
00073 this, SLOT( updateTopic() ));
00074
00075 alpha_property_ = new FloatProperty( "Alpha", 0.7,
00076 "Amount of transparency to apply to the map.",
00077 this, SLOT( updateAlpha() ));
00078 alpha_property_->setMin( 0 );
00079 alpha_property_->setMax( 1 );
00080
00081 draw_under_property_ = new Property( "Draw Behind", false,
00082 "Rendering option, controls whether or not the map is always"
00083 " drawn behind everything else.",
00084 this, SLOT( updateDrawUnder() ));
00085
00086 resolution_property_ = new FloatProperty( "Resolution", 0,
00087 "Resolution of the map. (not editable)", this );
00088 resolution_property_->setReadOnly( true );
00089
00090 width_property_ = new IntProperty( "Width", 0,
00091 "Width of the map, in meters. (not editable)", this );
00092 width_property_->setReadOnly( true );
00093
00094 height_property_ = new IntProperty( "Height", 0,
00095 "Height of the map, in meters. (not editable)", this );
00096 height_property_->setReadOnly( true );
00097
00098 position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO,
00099 "Position of the bottom left corner of the map, in meters. (not editable)",
00100 this );
00101 position_property_->setReadOnly( true );
00102
00103 orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY,
00104 "Orientation of the map. (not editable)",
00105 this );
00106 orientation_property_->setReadOnly( true );
00107 }
00108
00109 MapDisplay::~MapDisplay()
00110 {
00111 unsubscribe();
00112 clear();
00113 }
00114
00115 void MapDisplay::onInitialize()
00116 {
00117 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00118
00119 static int count = 0;
00120 std::stringstream ss;
00121 ss << "MapObjectMaterial" << count++;
00122 material_ = Ogre::MaterialManager::getSingleton().create( ss.str(),
00123 Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
00124 material_->setReceiveShadows(false);
00125 material_->getTechnique(0)->setLightingEnabled(false);
00126 material_->setDepthBias( -16.0f, 0.0f );
00127 material_->setCullingMode( Ogre::CULL_NONE );
00128 material_->setDepthWriteEnabled(false);
00129
00130 updateAlpha();
00131 }
00132
00133 void MapDisplay::onEnable()
00134 {
00135 subscribe();
00136 scene_node_->setVisible( true );
00137 }
00138
00139 void MapDisplay::onDisable()
00140 {
00141 unsubscribe();
00142 scene_node_->setVisible( false );
00143 clear();
00144 }
00145
00146 void MapDisplay::subscribe()
00147 {
00148 if ( !isEnabled() )
00149 {
00150 return;
00151 }
00152
00153 if( !topic_property_->getTopic().isEmpty() )
00154 {
00155 try
00156 {
00157 map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &MapDisplay::incomingMap, this );
00158 setStatus( StatusProperty::Ok, "Topic", "OK" );
00159 }
00160 catch( ros::Exception& e )
00161 {
00162 setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00163 }
00164 }
00165 }
00166
00167 void MapDisplay::unsubscribe()
00168 {
00169 map_sub_.shutdown();
00170 }
00171
00172 void MapDisplay::updateAlpha()
00173 {
00174 float alpha = alpha_property_->getFloat();
00175
00176 Ogre::Pass* pass = material_->getTechnique( 0 )->getPass( 0 );
00177 Ogre::TextureUnitState* tex_unit = NULL;
00178 if( pass->getNumTextureUnitStates() > 0 )
00179 {
00180 tex_unit = pass->getTextureUnitState( 0 );
00181 }
00182 else
00183 {
00184 tex_unit = pass->createTextureUnitState();
00185 }
00186
00187 tex_unit->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha );
00188
00189 if( alpha < 0.9998 )
00190 {
00191 material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00192 material_->setDepthWriteEnabled( false );
00193 }
00194 else
00195 {
00196 material_->setSceneBlending( Ogre::SBT_REPLACE );
00197 material_->setDepthWriteEnabled( !draw_under_property_->getValue().toBool() );
00198 }
00199 }
00200
00201 void MapDisplay::updateDrawUnder()
00202 {
00203 bool draw_under = draw_under_property_->getValue().toBool();
00204
00205 if( alpha_property_->getFloat() >= 0.9998 )
00206 {
00207 material_->setDepthWriteEnabled( !draw_under );
00208 }
00209
00210 if( manual_object_ )
00211 {
00212 if( draw_under )
00213 {
00214 manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_4 );
00215 }
00216 else
00217 {
00218 manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN );
00219 }
00220 }
00221 }
00222
00223 void MapDisplay::updateTopic()
00224 {
00225 unsubscribe();
00226 subscribe();
00227 clear();
00228 }
00229
00230 void MapDisplay::clear()
00231 {
00232 setStatus( StatusProperty::Warn, "Message", "No map received" );
00233
00234 if( !loaded_ )
00235 {
00236 return;
00237 }
00238
00239 scene_manager_->destroyManualObject( manual_object_ );
00240 manual_object_ = NULL;
00241
00242 std::string tex_name = texture_->getName();
00243 texture_.setNull();
00244 Ogre::TextureManager::getSingleton().unload( tex_name );
00245
00246 loaded_ = false;
00247 }
00248
00249 bool validateFloats(const nav_msgs::OccupancyGrid& msg)
00250 {
00251 bool valid = true;
00252 valid = valid && validateFloats( msg.info.resolution );
00253 valid = valid && validateFloats( msg.info.origin );
00254 return valid;
00255 }
00256
00257 void MapDisplay::incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
00258 {
00259 if( !validateFloats( *msg ))
00260 {
00261 setStatus( StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or infs)" );
00262 return;
00263 }
00264
00265 if( msg->info.width * msg->info.height == 0 )
00266 {
00267 std::stringstream ss;
00268 ss << "Map is zero-sized (" << msg->info.width << "x" << msg->info.height << ")";
00269 setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
00270 return;
00271 }
00272
00273 clear();
00274
00275 setStatus( StatusProperty::Ok, "Message", "Map received" );
00276
00277 ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n",
00278 msg->info.width,
00279 msg->info.height,
00280 msg->info.resolution );
00281
00282 float resolution = msg->info.resolution;
00283
00284 int width = msg->info.width;
00285 int height = msg->info.height;
00286
00287 map_ = msg;
00288 Ogre::Vector3 position( msg->info.origin.position.x,
00289 msg->info.origin.position.y,
00290 msg->info.origin.position.z );
00291 Ogre::Quaternion orientation( msg->info.origin.orientation.w,
00292 msg->info.origin.orientation.x,
00293 msg->info.origin.orientation.y,
00294 msg->info.origin.orientation.z );
00295 frame_ = msg->header.frame_id;
00296 if (frame_.empty())
00297 {
00298 frame_ = "/map";
00299 }
00300
00301
00302 unsigned int pixels_size = width * height;
00303 unsigned char* pixels = new unsigned char[pixels_size];
00304 memset(pixels, 255, pixels_size);
00305
00306 bool map_status_set = false;
00307 unsigned int num_pixels_to_copy = pixels_size;
00308 if( pixels_size != msg->data.size() )
00309 {
00310 std::stringstream ss;
00311 ss << "Data size doesn't match width*height: width = " << width
00312 << ", height = " << height << ", data size = " << msg->data.size();
00313 setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
00314 map_status_set = true;
00315
00316
00317 if( msg->data.size() < pixels_size )
00318 {
00319 num_pixels_to_copy = msg->data.size();
00320 }
00321 }
00322
00323 for( unsigned int pixel_index = 0; pixel_index < num_pixels_to_copy; pixel_index++ )
00324 {
00325 unsigned char val;
00326 if(msg->data[ pixel_index ] == 100)
00327 val = 0;
00328 else if(msg->data[ pixel_index ] == 0)
00329 val = 255;
00330 else
00331 val = 127;
00332
00333 pixels[ pixel_index ] = val;
00334 }
00335
00336 Ogre::DataStreamPtr pixel_stream;
00337 pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size ));
00338 static int tex_count = 0;
00339 std::stringstream ss;
00340 ss << "MapTexture" << tex_count++;
00341 try
00342 {
00343 texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00344 pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D,
00345 0);
00346
00347 if( !map_status_set )
00348 {
00349 setStatus( StatusProperty::Ok, "Map", "Map OK" );
00350 }
00351 }
00352 catch(Ogre::RenderingAPIException&)
00353 {
00354 Ogre::Image image;
00355 pixel_stream->seek(0);
00356 float fwidth = width;
00357 float fheight = height;
00358 if( width > height )
00359 {
00360 float aspect = fheight / fwidth;
00361 fwidth = 2048;
00362 fheight = fwidth * aspect;
00363 }
00364 else
00365 {
00366 float aspect = fwidth / fheight;
00367 fheight = 2048;
00368 fwidth = fheight * aspect;
00369 }
00370
00371 {
00372 std::stringstream ss;
00373 ss << "Map is larger than your graphics card supports. Downsampled from [" << width << "x" << height << "] to [" << fwidth << "x" << fheight << "]";
00374 setStatus(StatusProperty::Ok, "Map", QString::fromStdString( ss.str() ));
00375 }
00376
00377 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);
00378
00379 image.loadRawData(pixel_stream, width, height, Ogre::PF_L8);
00380 image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST);
00381 ss << "Downsampled";
00382 texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
00383 }
00384
00385 delete [] pixels;
00386
00387 Ogre::Pass* pass = material_->getTechnique(0)->getPass(0);
00388 Ogre::TextureUnitState* tex_unit = NULL;
00389 if (pass->getNumTextureUnitStates() > 0)
00390 {
00391 tex_unit = pass->getTextureUnitState(0);
00392 }
00393 else
00394 {
00395 tex_unit = pass->createTextureUnitState();
00396 }
00397
00398 tex_unit->setTextureName(texture_->getName());
00399 tex_unit->setTextureFiltering( Ogre::TFO_NONE );
00400
00401 static int map_count = 0;
00402 std::stringstream ss2;
00403 ss2 << "MapObject" << map_count++;
00404 manual_object_ = scene_manager_->createManualObject( ss2.str() );
00405 scene_node_->attachObject( manual_object_ );
00406
00407 manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
00408 {
00409
00410 {
00411
00412 manual_object_->position( 0.0f, 0.0f, 0.0f );
00413 manual_object_->textureCoord(0.0f, 0.0f);
00414 manual_object_->normal( 0.0f, 0.0f, 1.0f );
00415
00416
00417 manual_object_->position( resolution*width, resolution*height, 0.0f );
00418 manual_object_->textureCoord(1.0f, 1.0f);
00419 manual_object_->normal( 0.0f, 0.0f, 1.0f );
00420
00421
00422 manual_object_->position( 0.0f, resolution*height, 0.0f );
00423 manual_object_->textureCoord(0.0f, 1.0f);
00424 manual_object_->normal( 0.0f, 0.0f, 1.0f );
00425 }
00426
00427
00428 {
00429
00430 manual_object_->position( 0.0f, 0.0f, 0.0f );
00431 manual_object_->textureCoord(0.0f, 0.0f);
00432 manual_object_->normal( 0.0f, 0.0f, 1.0f );
00433
00434
00435 manual_object_->position( resolution*width, 0.0f, 0.0f );
00436 manual_object_->textureCoord(1.0f, 0.0f);
00437 manual_object_->normal( 0.0f, 0.0f, 1.0f );
00438
00439
00440 manual_object_->position( resolution*width, resolution*height, 0.0f );
00441 manual_object_->textureCoord(1.0f, 1.0f);
00442 manual_object_->normal( 0.0f, 0.0f, 1.0f );
00443 }
00444 }
00445 manual_object_->end();
00446
00447 if( draw_under_property_->getValue().toBool() )
00448 {
00449 manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4);
00450 }
00451
00452 resolution_property_->setValue( resolution );
00453 width_property_->setValue( width );
00454 height_property_->setValue( height );
00455 position_property_->setVector( position );
00456 orientation_property_->setQuaternion( orientation );
00457
00458 transformMap();
00459
00460 loaded_ = true;
00461
00462 context_->queueRender();
00463 }
00464
00465 void MapDisplay::transformMap()
00466 {
00467 if (!map_)
00468 {
00469 return;
00470 }
00471
00472 Ogre::Vector3 position;
00473 Ogre::Quaternion orientation;
00474 if (!context_->getFrameManager()->transform(frame_, ros::Time(), map_->info.origin, position, orientation))
00475 {
00476 ROS_DEBUG( "Error transforming map '%s' from frame '%s' to frame '%s'",
00477 qPrintable( getName() ), frame_.c_str(), qPrintable( fixed_frame_ ));
00478
00479 setStatus( StatusProperty::Error, "Transform",
00480 "No transform from [" + QString::fromStdString( frame_ ) + "] to [" + fixed_frame_ + "]" );
00481 }
00482 else
00483 {
00484 setStatus(StatusProperty::Ok, "Transform", "Transform OK");
00485 }
00486
00487 scene_node_->setPosition( position );
00488 scene_node_->setOrientation( orientation );
00489 }
00490
00491 void MapDisplay::fixedFrameChanged()
00492 {
00493 transformMap();
00494 }
00495
00496 void MapDisplay::reset()
00497 {
00498 Display::reset();
00499
00500 clear();
00501
00502 updateTopic();
00503 }
00504
00505 }
00506
00507 #include <pluginlib/class_list_macros.h>
00508 PLUGINLIB_DECLARE_CLASS( rviz, Map, rviz::MapDisplay, rviz::Display )