rviz_interaction_tools: image_overlay.cpp Source File

00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "rviz_interaction_tools/image_overlay.h"
00031 #include "rviz_interaction_tools/image_tools.h"
00032 #include "rviz_interaction_tools/unique_string_manager.h"
00033 
00034 #include <sensor_msgs/image_encodings.h>
00035 
00036 #include <vector>
00037 
00038 #include <OGRE/OgreSceneNode.h>
00039 #include <OGRE/OgreRectangle2D.h>
00040 #include <OGRE/OgreMaterialManager.h>
00041 #include <OGRE/OgreTextureManager.h>
00042 #include <OGRE/OgreTechnique.h>
00043 
00044 using namespace Ogre;
00045 
00046 namespace rviz_interaction_tools
00047 {
00048 
00049 ImageOverlay::ImageOverlay( Ogre::SceneNode* scene_root, unsigned char render_queue_group ) :
00050   scene_root_(scene_root),
00051   new_image_(false),
00052   width_(0),
00053   height_(0)
00054 {
00055   //create empty image
00056   Ogre::PixelFormat format = Ogre::PF_BYTE_RGB;
00057   std::vector<unsigned char> empty_buffer;
00058 
00059   empty_buffer.assign(100 * 100 * 3, 0);
00060   void* data_ptr = (void*) &empty_buffer[0];
00061   uint32_t data_size = empty_buffer.size();
00062 
00063   Ogre::DataStreamPtr pixel_stream;
00064   pixel_stream.bind(new Ogre::MemoryDataStream(data_ptr, data_size));
00065 
00066   try
00067   {
00068     empty_image_.loadRawData(pixel_stream, 100, 100, 1, format, 1, 0);
00069   } catch (Ogre::Exception& e)
00070   {
00071     ROS_ERROR("Error loading image: %s", e.what());
00072   }
00073 
00074   UniqueStringManager unique_string_mgr;
00075 
00076   resource_group_name_ = unique_string_mgr.unique("rviz_interaction_tools::ImageOverlay");
00077   Ogre::ResourceGroupManager::getSingleton().createResourceGroup(resource_group_name_);
00078   Ogre::ResourceGroupManager::getSingleton().initialiseResourceGroup(resource_group_name_);
00079 
00080   //prepare empty texture
00081   texture_ = Ogre::TextureManager::getSingleton().loadImage(
00082       unique_string_mgr.unique("ImageOverlayTexture"),
00083       resource_group_name_, empty_image_,
00084       Ogre::TEX_TYPE_2D, 0);
00085 
00086   //prepare material
00087   texture_material_ = Ogre::MaterialManager::getSingleton().create(
00088       unique_string_mgr.unique("ImageOverlayMaterial"),
00089       resource_group_name_);
00090 
00091   Ogre::TextureUnitState* texture_unit =
00092       texture_material_->getTechnique(0)->getPass(0)->createTextureUnitState();
00093   texture_unit->setTextureName(texture_->getName());
00094   texture_unit->setTextureFiltering(Ogre::TFO_NONE);
00095 
00096   texture_material_->setSceneBlending(Ogre::SBT_REPLACE);
00097   texture_material_->setDepthWriteEnabled(false);
00098   texture_material_->setReceiveShadows(false);
00099   texture_material_->setDepthCheckEnabled(false);
00100   texture_material_->getTechnique(0)->setLightingEnabled(false);
00101   texture_material_->setCullingMode(Ogre::CULL_NONE);
00102 
00103   image_rect_ = new Ogre::Rectangle2D(true);
00104   //image texture will be rendered after point cloud, but before any other geometry
00105   image_rect_->setRenderQueueGroup(render_queue_group);
00106   image_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00107   image_rect_->setMaterial(texture_material_->getName());
00108   image_rect_->setQueryFlags(0);
00109 
00110   Ogre::AxisAlignedBox aabInf;
00111   aabInf.setInfinite();
00112   image_rect_->setBoundingBox(aabInf);
00113 
00114   scene_root_->attachObject(image_rect_);
00115 }
00116 
00117 ImageOverlay::~ImageOverlay()
00118 {
00119   Ogre::ResourceGroupManager::getSingleton().destroyResourceGroup(resource_group_name_);
00120   scene_root_->detachObject(image_rect_);
00121   delete image_rect_;
00122 }
00123 
00124 bool ImageOverlay::setImage( const sensor_msgs::Image &image,
00125     const stereo_msgs::DisparityImage &disparity_image )
00126 {
00127   boost::mutex::scoped_lock lock(mutex_);
00128 
00129   if (!setImageNoLock(image))
00130   {
00131     return false;
00132   }
00133 
00134   if ((image.width != disparity_image.image.width) || (image.height
00135       != disparity_image.image.height))
00136   {
00137     ROS_ERROR("Size mismatch between image (%i x %i) and disparity image (%i x %i)!",
00138         image.width, image.height, disparity_image.image.width, disparity_image.image.height );
00139     return false;
00140   }
00141 
00142   //paint over the parts with no depth info
00143   for (unsigned int i = 0; i < image.height; i++)
00144   {
00145     for (unsigned int j = 0; j < image.width; j++)
00146     {
00147       unsigned int adr = i * image.width + j;
00148       if (!rviz_interaction_tools::hasDisparityValue(disparity_image, i, j))
00149       {
00150         if (image_buffer_[3 * adr + 0] < 215)
00151           image_buffer_[3 * adr + 0] += 40;
00152         else
00153           image_buffer_[3 * adr + 0] = 255;
00154         if (image_buffer_[3 * adr + 1] > 200)
00155           image_buffer_[3 * adr + 1] = 200;
00156         if (image_buffer_[3 * adr + 2] > 200)
00157           image_buffer_[3 * adr + 2] = 200;
00158       }
00159     }
00160   }
00161 
00162   new_image_ = true;
00163 
00164   return true;
00165 }
00166 
00167 bool ImageOverlay::setImage( const sensor_msgs::Image &image )
00168 {
00169   boost::mutex::scoped_lock lock(mutex_);
00170 
00171   if (!setImageNoLock(image))
00172   {
00173     return false;
00174   }
00175 
00176   new_image_ = true;
00177   return true;
00178 }
00179 
00180 bool ImageOverlay::setImageNoLock( const sensor_msgs::Image &image )
00181 {
00182   width_ = image.width;
00183   height_ = image.height;
00184 
00185   //copy image to a new buffer
00186   image_buffer_.resize(3 * width_ * height_, 0);
00187 
00188   if (image.encoding == sensor_msgs::image_encodings::MONO8)
00189   {
00190     for (unsigned int i = 0; i < image.height; i++)
00191     {
00192       for (unsigned int j = 0; j < image.width; j++)
00193       {
00194         unsigned int adr = i * image.width + j;
00195         image_buffer_[3 * adr + 0] = image.data[adr];
00196         image_buffer_[3 * adr + 1] = image.data[adr];
00197         image_buffer_[3 * adr + 2] = image.data[adr];
00198       }
00199     }
00200   }
00201   else if (image.encoding == sensor_msgs::image_encodings::BGR8)
00202   {
00203     for (unsigned int i = 0; i < image.height; i++)
00204     {
00205       for (unsigned int j = 0; j < image.width; j++)
00206       {
00207         unsigned int adr = i * image.width + j;
00208         unsigned int adr2 = i * image.step + j*3;
00209         image_buffer_[3 * adr + 0] = image.data[adr2 + 2];
00210         image_buffer_[3 * adr + 1] = image.data[adr2 + 1];
00211         image_buffer_[3 * adr + 2] = image.data[adr2 + 0];
00212       }
00213     }
00214   }
00215   else if (image.encoding == sensor_msgs::image_encodings::RGB8)
00216   {
00217     image_buffer_ = image.data;
00218   }
00219   else
00220   {
00221     ROS_ERROR("ImageDisplay only supports MONO8, RGB8 and BGR8 images");
00222     return false;
00223   }
00224 
00225   return true;
00226 }
00227 
00228 bool ImageOverlay::setImage( unsigned char *rgb_data, int width, int height )
00229 {
00230   boost::mutex::scoped_lock lock(mutex_);
00231 
00232   if (width <= 0 || height <= 0)
00233   {
00234     ROS_ERROR( "Image dimensions must be > 0" );
00235     return false;
00236   }
00237 
00238   width_ = width;
00239   height_ = height;
00240 
00241   image_buffer_.resize(width * height * 3, 0);
00242   memcpy(&image_buffer_[0], rgb_data, width * height * 3);
00243 
00244   return true;
00245 }
00246 
00247 bool ImageOverlay::update()
00248 {
00249   boost::mutex::scoped_lock lock(mutex_);
00250 
00251   if (!new_image_ || !width_ || image_buffer_.size() != unsigned(width_
00252       * height_ * 3))
00253   {
00254     return false;
00255   }
00256 
00257   Ogre::PixelFormat format = Ogre::PF_BYTE_RGB;
00258 
00259   void* data_ptr = (void*) &image_buffer_[0];
00260   uint32_t data_size = image_buffer_.size();
00261 
00262   Ogre::DataStreamPtr pixel_stream;
00263   pixel_stream.bind(new Ogre::MemoryDataStream(data_ptr, data_size));
00264 
00265   Ogre::Image ogre_image;
00266 
00267   try
00268   {
00269     ogre_image.loadRawData(pixel_stream, width_, height_, 1, format, 1, 0);
00270   } catch (Ogre::Exception& e)
00271   {
00272     ROS_ERROR("Error loading image: %s", e.what());
00273     return false;
00274   }
00275 
00276   texture_->unload();
00277   texture_->loadImage(ogre_image);
00278 
00279   return true;
00280 }
00281 
00282 void ImageOverlay::clear()
00283 {
00284   boost::mutex::scoped_lock lock(mutex_);
00285 
00286   texture_->unload();
00287 
00288   new_image_ = false;
00289 }
00290 
00291 int ImageOverlay::getWidth()
00292 {
00293   boost::mutex::scoped_lock lock(mutex_);
00294   int w = width_;
00295   return w;
00296 }
00297 
00298 int ImageOverlay::getHeight()
00299 {
00300   boost::mutex::scoped_lock lock(mutex_);
00301   int h = height_;
00302   return h;
00303 }
00304 
00305 }