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