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