void OnePixelMaterialGenerator::loadResource(Ogre::Resource* resource)
{
Ogre::Texture* texture = static_cast<Ogre::Texture*>(resource);
static Ogre::uchar data[3] = {0xFF, 0x7F, 0x7F};
Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(&data, 3, false, true));
Ogre::Image image;
image.loadRawData(stream, 1,1,1, Ogre::PF_R8G8B8);
Ogre::ConstImagePtrList images({&image});
texture->_loadImages(images);
}
void MapDisplay::showMap()
{
if (current_map_.data.empty())
{
return;
}
if( !validateFloats( current_map_ ))
{
setStatus( StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or infs)" );
return;
}
if( current_map_.info.width * current_map_.info.height == 0 )
{
std::stringstream ss;
ss << "Map is zero-sized (" << current_map_.info.width << "x" << current_map_.info.height << ")";
setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
return;
}
setStatus( StatusProperty::Ok, "Message", "Map received" );
ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n",
current_map_.info.width,
current_map_.info.height,
current_map_.info.resolution );
float resolution = current_map_.info.resolution;
int width = current_map_.info.width;
int height = current_map_.info.height;
Ogre::Vector3 position( current_map_.info.origin.position.x,
current_map_.info.origin.position.y,
current_map_.info.origin.position.z );
Ogre::Quaternion orientation( current_map_.info.origin.orientation.w,
current_map_.info.origin.orientation.x,
current_map_.info.origin.orientation.y,
current_map_.info.origin.orientation.z );
frame_ = current_map_.header.frame_id;
if (frame_.empty())
{
frame_ = "/map";
}
unsigned int pixels_size = width * height;
unsigned char* pixels = new unsigned char[pixels_size];
memset(pixels, 255, pixels_size);
bool map_status_set = false;
unsigned int num_pixels_to_copy = pixels_size;
if( pixels_size != current_map_.data.size() )
{
std::stringstream ss;
ss << "Data size doesn't match width*height: width = " << width
<< ", height = " << height << ", data size = " << current_map_.data.size();
setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
map_status_set = true;
// Keep going, but don't read past the end of the data.
if( current_map_.data.size() < pixels_size )
{
num_pixels_to_copy = current_map_.data.size();
}
}
memcpy( pixels, ¤t_map_.data[0], num_pixels_to_copy );
Ogre::DataStreamPtr pixel_stream;
pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size ));
if( !texture_.isNull() )
{
Ogre::TextureManager::getSingleton().remove( texture_->getName() );
texture_.setNull();
}
static int tex_count = 0;
std::stringstream ss;
ss << "MapTexture" << tex_count++;
try
{
texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D,
0);
if( !map_status_set )
{
setStatus( StatusProperty::Ok, "Map", "Map OK" );
}
}
catch(Ogre::RenderingAPIException&)
{
Ogre::Image image;
pixel_stream->seek(0);
float fwidth = width;
float fheight = height;
if( width > height )
{
float aspect = fheight / fwidth;
fwidth = 2048;
fheight = fwidth * aspect;
}
else
{
float aspect = fwidth / fheight;
fheight = 2048;
fwidth = fheight * aspect;
}
{
std::stringstream ss;
ss << "Map is larger than your graphics card supports. Downsampled from [" << width << "x" << height << "] to [" << fwidth << "x" << fheight << "]";
setStatus(StatusProperty::Ok, "Map", QString::fromStdString( ss.str() ));
}
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);
//ROS_INFO("Stream size [%d], width [%f], height [%f], w * h [%f]", pixel_stream->size(), width, height, width * height);
image.loadRawData(pixel_stream, width, height, Ogre::PF_L8);
image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST);
ss << "Downsampled";
texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
}
delete [] pixels;
Ogre::Pass* pass = material_->getTechnique(0)->getPass(0);
Ogre::TextureUnitState* tex_unit = NULL;
if (pass->getNumTextureUnitStates() > 0)
{
tex_unit = pass->getTextureUnitState(0);
}
else
{
tex_unit = pass->createTextureUnitState();
}
tex_unit->setTextureName(texture_->getName());
tex_unit->setTextureFiltering( Ogre::TFO_NONE );
updatePalette();
resolution_property_->setValue( resolution );
width_property_->setValue( width );
height_property_->setValue( height );
position_property_->setVector( position );
orientation_property_->setQuaternion( orientation );
transformMap();
manual_object_->setVisible( true );
scene_node_->setScale( resolution * width, resolution * height, 1.0 );
context_->queueRender();
}
void NavViewPanel::displayMap(const nav_msgs::OccupancyGrid& map)
{
boost::mutex::scoped_lock lock(map_lock_);
map_resolution_ = map.info.resolution;
// Pad dimensions to power of 2
map_width_ = map.info.width;//(int)pow(2,ceil(log2(map.info.width)));
map_height_ = map.info.height;//(int)pow(2,ceil(log2(map.info.height)));
map_origin_x_ = map.info.origin.position.x;
map_origin_y_ = map.info.origin.position.y;
//ROS_INFO("Padded dimensions to %d X %d\n", map_width_, map_height_);
// Expand it to be RGB data
int pixels_size = map_width_ * map_height_ * 3;
unsigned char* pixels = new unsigned char[pixels_size];
memset(pixels, 255, pixels_size);
for(unsigned int j=0;j<map.info.height;j++)
{
for(unsigned int i=0;i<map.info.width;i++)
{
unsigned char val;
if(map.data[j*map.info.width+i] == 100)
val = 0;
else if(map.data[j*map.info.width+i] == 0)
val = 255;
else
val = 127;
int pidx = 3*(j*map_width_ + i);
pixels[pidx+0] = val;
pixels[pidx+1] = val;
pixels[pidx+2] = val;
}
}
Ogre::DataStreamPtr pixel_stream;
pixel_stream.bind(new Ogre::MemoryDataStream( pixels, pixels_size ));
static int tex_count = 0;
std::stringstream ss;
ss << "NavViewMapTexture" << tex_count++;
try
{
map_texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
pixel_stream, map_width_, map_height_, Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D,
0);
}
catch(Ogre::RenderingAPIException&)
{
Ogre::Image image;
pixel_stream->seek(0);
float width = map_width_;
float height = map_height_;
if (map_width_ > map_height_)
{
float aspect = height / width;
width = 2048;
height = width * aspect;
}
else
{
float aspect = width / height;
height = 2048;
width = height * aspect;
}
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);
image.loadRawData(pixel_stream, map_width_, map_height_, Ogre::PF_BYTE_RGB);
image.resize(width, height, Ogre::Image::FILTER_NEAREST);
ss << "Downsampled";
map_texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
}
delete [] pixels;
Ogre::SceneNode* map_node = NULL;
if ( !map_object_ )
{
static int map_count = 0;
std::stringstream ss;
ss << "NavViewMapObject" << map_count++;
map_object_ = scene_manager_->createManualObject( ss.str() );
map_node = root_node_->createChildSceneNode();
map_node->attachObject( map_object_ );
ss << "Material";
map_material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
map_material_->setReceiveShadows(false);
map_material_->getTechnique(0)->setLightingEnabled(false);
}
else
{
map_node = map_object_->getParentSceneNode();
}
Ogre::Pass* pass = map_material_->getTechnique(0)->getPass(0);
Ogre::TextureUnitState* tex_unit = NULL;
if (pass->getNumTextureUnitStates() > 0)
{
tex_unit = pass->getTextureUnitState(0);
}
else
{
tex_unit = pass->createTextureUnitState();
}
tex_unit->setTextureName(map_texture_->getName());
tex_unit->setTextureFiltering( Ogre::TFO_NONE );
map_object_->clear();
map_object_->begin(map_material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
{
// First triangle
{
// Top left
map_object_->position( 0.0f, 0.0f, 0.0f );
map_object_->textureCoord(0.0f, 0.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
// Bottom right
map_object_->position( map_resolution_*map_width_, map_resolution_*map_height_, 0.0f );
map_object_->textureCoord(1.0f, 1.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
// Bottom left
map_object_->position( 0.0f, map_resolution_*map_height_, 0.0f );
map_object_->textureCoord(0.0f, 1.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
}
// Second triangle
{
// Top left
map_object_->position( 0.0f, 0.0f, 0.0f );
map_object_->textureCoord(0.0f, 0.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
// Top right
map_object_->position( map_resolution_*map_width_, 0.0f, 0.0f );
map_object_->textureCoord(1.0f, 0.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
// Bottom right
map_object_->position( map_resolution_*map_width_, map_resolution_*map_height_, 0.0f );
map_object_->textureCoord(1.0f, 1.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
}
}
map_object_->end();
//only center the position of the map if its the first received
if(first_map_){
root_node_->setPosition(Ogre::Vector3(-map_width_*map_resolution_/2, -map_height_*map_resolution_/2, 0.0f));
first_map_ = false;
}
map_node->setPosition(Ogre::Vector3(0.0f, 0.0f, MAP_DEPTH));
queueRender();
}
void Terrain::resourcesLoaded()
{
mTerrain = OGRE_NEW Ogre::Terrain( GlobalsBase::mScene );
Ogre::Terrain::ImportData imp;
Ogre::Image img;
switch( mHeightmapType )
{
case HEIGHTMAPTYPE_IMAGE:
{
Ogre::TexturePtr texture = Ogre::TextureManager::getSingleton().getByName(
mHeightmapFile.string(), mResourceManager.getGroup() );
texture->convertToImage( img );
imp.inputImage = &img;
imp.terrainSize = img.getWidth();
imp.inputScale = mInputScale;
break;
}
case HEIGHTMAPTYPE_RAW:
{
Ogre::DataStreamPtr stream = Ogre::ResourceGroupManager::getSingleton().openResource(
mHeightmapFile.string(), mResourceManager.getGroup() );
img.loadRawData( stream, 1025, 1025, Ogre::PF_FLOAT32_R );
imp.inputImage = &img;
imp.terrainSize = img.getWidth();
imp.inputScale = 1;
break;
}
case HEIGHTMAPTYPE_OGREBINARY:
break;
}
if( mTerrainType != TERRAINTYPE_OGREBINARY )
{
// Generic import data
imp.worldSize = 1025;
imp.minBatchSize = 33;
imp.maxBatchSize = 65;
imp.pos = toVector3<Ogre::Vector3>(
ServerPosition( ClientPlugin::getServer().getGridPosition() ).get3DPosition() );
mTerrain->setPosition( imp.pos );
// Set textures
imp.layerList.resize( mLayerInstances.size() );
for( unsigned int i = 0; i < mLayerInstances.size(); ++i )
{
imp.layerList[i].worldSize = mLayerInstances[i].mWorldSize;
for( std::vector<Path>::iterator j = mLayerInstances[i].mTextureFiles.begin();
j != mLayerInstances[i].mTextureFiles.end(); ++j )
{
imp.layerList[i].textureNames.push_back( (*j).string() );
}
}
// Load the terrain
mTerrain->prepare( imp );
mTerrain->load();
}
// Set blending
switch( mTerrainType )
{
case TERRAINTYPE_AUTOBLENDMAP:
{
Ogre::TerrainLayerBlendMap* blendMap0 = mTerrain->getLayerBlendMap(1);
Ogre::TerrainLayerBlendMap* blendMap1 = mTerrain->getLayerBlendMap(2);
float* pBlend1 = blendMap1->getBlendPointer();
for (Ogre::uint16 y = 0; y < mTerrain->getLayerBlendMapSize(); ++y)
{
for (Ogre::uint16 x = 0; x < mTerrain->getLayerBlendMapSize(); ++x)
{
Ogre::Real tx, ty;
blendMap0->convertImageToTerrainSpace(x, y, &tx, &ty);
Ogre::Real height = mTerrain->getHeightAtTerrainPosition(tx, ty);
Ogre::Real val = (height - mMinHeight0) / mFadeDistance0;
val = Ogre::Math::Clamp(val, (Ogre::Real)0, (Ogre::Real)1);
val = (height - mMinHeight1) / mFadeDistance1;
val = Ogre::Math::Clamp(val, (Ogre::Real)0, (Ogre::Real)1);
*pBlend1++ = val;
}
}
blendMap0->dirty();
blendMap1->dirty();
blendMap0->update();
blendMap1->update();
break;
}
case TERRAINTYPE_OGREBINARY:
// Everything is embedded in the binary file.
mTerrain->setResourceGroup( mResourceManager.getGroup() );
mTerrain->prepare( mHeightmapFile.string() );
mTerrain->load( mHeightmapFile.string() );
break;
}
mTerrain->freeTemporaryResources();
ClientPlugin::mLoadingCompletedSignal( *this );
}
