ros_image_texture.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, 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 <map>
00031 
00032 #include <boost/algorithm/string/erase.hpp>
00033 #include <boost/foreach.hpp>
00034 
00035 #include <OGRE/OgreTextureManager.h>
00036 
00037 #include <pluginlib/class_loader.h>
00038 
00039 #include <image_transport/subscriber_plugin.h>
00040 
00041 #include <sensor_msgs/image_encodings.h>
00042 
00043 #include <tf/tf.h>
00044 
00045 #include "rviz/uniform_string_stream.h"
00046 
00047 #include "rviz/image/ros_image_texture.h"
00048 
00049 namespace rviz
00050 {
00051 
00052 ROSImageTexture::ROSImageTexture(const ros::NodeHandle& nh)
00053 : nh_(nh)
00054 , it_(nh_)
00055 , transport_type_("raw")
00056 , new_image_(false)
00057 , width_(0)
00058 , height_(0)
00059 , tf_client_(0)
00060 , image_count_(0)
00061 , queue_size_(2)
00062 {
00063   empty_image_.load("no_image.png", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00064 
00065   static uint32_t count = 0;
00066   UniformStringStream ss;
00067   ss << "ROSImageTexture" << count++;
00068   texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, empty_image_, Ogre::TEX_TYPE_2D, 0);
00069 
00070   scanForTransportSubscriberPlugins();
00071 }
00072 
00073 ROSImageTexture::~ROSImageTexture()
00074 {
00075   current_image_.reset();
00076 }
00077 
00078 void ROSImageTexture::clear()
00079 {
00080   boost::mutex::scoped_lock lock(mutex_);
00081 
00082   texture_->unload();
00083   texture_->loadImage(empty_image_);
00084 
00085   new_image_ = false;
00086   current_image_.reset();
00087 
00088   if (tf_filter_)
00089   {
00090     tf_filter_->clear();
00091   }
00092 
00093   image_count_ = 0;
00094 }
00095 
00096 void ROSImageTexture::setFrame(const std::string& frame, tf::TransformListener* tf_client)
00097 {
00098   tf_client_ = tf_client;
00099   frame_ = frame;
00100   setTopic(topic_);
00101 }
00102 
00103 void ROSImageTexture::setTopic(const std::string& topic)
00104 {
00105   boost::mutex::scoped_lock lock(mutex_);
00106   // Must reset the current image here because image_transport will unload the plugin as soon as we unsubscribe,
00107   // which removes the code segment necessary for the shared_ptr's deleter to exist!
00108   current_image_.reset();
00109 
00110   topic_ = topic;
00111   tf_filter_.reset();
00112 
00113   sub_.reset(new image_transport::SubscriberFilter());
00114 
00115   if (!topic.empty())
00116   {
00117     sub_->subscribe(it_, topic, 1, image_transport::TransportHints(transport_type_));
00118 
00119     if (frame_.empty())
00120     {
00121       sub_->registerCallback(boost::bind(&ROSImageTexture::callback, this, _1));
00122     }
00123     else
00124     {
00125       ROS_ASSERT(tf_client_);
00126       tf_filter_.reset(new tf::MessageFilter<sensor_msgs::Image>(*sub_, (tf::Transformer&)*tf_client_, frame_, queue_size_, nh_));
00127       tf_filter_->registerCallback(boost::bind(&ROSImageTexture::callback, this, _1));
00128     }
00129   }
00130 }
00131 
00132 void ROSImageTexture::setTransportType(const std::string& transport_type)
00133 {
00134   transport_type_ = transport_type;
00135   setTopic(topic_);
00136 }
00137 
00138 void ROSImageTexture::getAvailableTransportTypes(V_string& types)
00139 {
00140   types.push_back("raw");
00141 
00142   // Loop over all current ROS topic names
00143   ros::master::V_TopicInfo topics;
00144   ros::master::getTopics(topics);
00145   ros::master::V_TopicInfo::iterator it = topics.begin();
00146   ros::master::V_TopicInfo::iterator end = topics.end();
00147   for (; it != end; ++it)
00148   {
00149     // If the beginning of this topic name is the same as topic_,
00150     // and the whole string is not the same,
00151     // and the next character is /
00152     // and there are no further slashes from there to the end,
00153     // then consider this a possible transport topic.
00154     const ros::master::TopicInfo& ti = *it;
00155     const std::string& topic_name = ti.name;
00156     if( topic_name.find( topic_ ) == 0 &&
00157         topic_name != topic_ &&
00158         topic_name[ topic_.size() ] == '/' &&
00159         topic_name.find( '/', topic_.size() + 1 ) == std::string::npos )
00160     {
00161       std::string transport_type = topic_name.substr( topic_.size() + 1 );
00162 
00163       // If the transport type string found above is in the set of
00164       // supported transport type plugins, add it to the list.
00165       if( transport_plugin_types_.find( transport_type ) != transport_plugin_types_.end() )
00166       {
00167         types.push_back( transport_type );
00168       }
00169     }
00170   }
00171 }
00172 
00173 const sensor_msgs::Image::ConstPtr& ROSImageTexture::getImage()
00174 {
00175   boost::mutex::scoped_lock lock(mutex_);
00176 
00177   return current_image_;
00178 }
00179 
00180 void ROSImageTexture::setQueueSize( int size )
00181 {
00182   queue_size_ = size;
00183   if( tf_filter_ )
00184   {
00185     tf_filter_->setQueueSize( (uint32_t) size );
00186   }
00187 }
00188 
00189 int ROSImageTexture::getQueueSize()
00190 {
00191   return queue_size_;
00192 }
00193 
00194 bool ROSImageTexture::update()
00195 {
00196   sensor_msgs::Image::ConstPtr image;
00197   bool new_image = false;
00198   {
00199     boost::mutex::scoped_lock lock(mutex_);
00200 
00201     image = current_image_;
00202     new_image = new_image_;
00203   }
00204 
00205   if (!image || !new_image)
00206   {
00207     return false;
00208   }
00209 
00210   new_image_ = false;
00211 
00212   if (image->data.empty())
00213   {
00214     return false;
00215   }
00216 
00217   Ogre::PixelFormat format = Ogre::PF_R8G8B8;
00218   Ogre::Image ogre_image;
00219   std::vector<uint8_t> buffer;
00220   void* data_ptr = (void*)&image->data[0];
00221   uint32_t data_size = image->data.size();
00222   if (image->encoding == sensor_msgs::image_encodings::RGB8)
00223   {
00224     format = Ogre::PF_BYTE_RGB;
00225   }
00226   else if (image->encoding == sensor_msgs::image_encodings::RGBA8)
00227   {
00228     format = Ogre::PF_BYTE_RGBA;
00229   }
00230   else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC4 ||
00231            image->encoding == sensor_msgs::image_encodings::TYPE_8SC4 ||
00232            image->encoding == sensor_msgs::image_encodings::BGRA8)
00233   {
00234     format = Ogre::PF_BYTE_BGRA;
00235   }
00236   else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC3 ||
00237            image->encoding == sensor_msgs::image_encodings::TYPE_8SC3 ||
00238            image->encoding == sensor_msgs::image_encodings::BGR8)
00239   {
00240     format = Ogre::PF_BYTE_BGR;
00241   }
00242   else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC1 ||
00243            image->encoding == sensor_msgs::image_encodings::TYPE_8SC1 ||
00244            image->encoding == sensor_msgs::image_encodings::MONO8)
00245   {
00246     format = Ogre::PF_BYTE_L;
00247   }
00248   else if (image->encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
00249            image->encoding == sensor_msgs::image_encodings::TYPE_16SC1 ||
00250            image->encoding == sensor_msgs::image_encodings::MONO16)
00251   {
00252     format = Ogre::PF_BYTE_L;
00253 
00254     // downsample manually to 8-bit, because otherwise the lower 8-bits are simply removed
00255     buffer.resize(image->data.size() / 2);
00256     data_size = buffer.size();
00257     data_ptr = (void*)&buffer[0];
00258     for (size_t i = 0; i < data_size; ++i)
00259     {
00260       uint16_t s = image->data[2*i] << 8 | image->data[2*i + 1];
00261       float val = (float)s / std::numeric_limits<uint16_t>::max();
00262       buffer[i] = val * 255;
00263     }
00264   }
00265   else if (image->encoding.find("bayer") == 0)
00266   {
00267     format = Ogre::PF_BYTE_L;
00268   }
00269   else if (image->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
00270   {
00271     // Ogre encoding
00272     format = Ogre::PF_FLOAT32_R;
00273 
00274     // Rescale floating point image to 0<=x<=1 scale
00275     size_t i;
00276     float* img_ptr;
00277     uint32_t img_size = image->data.size() / sizeof(float);
00278 
00279     // Find min. and max. pixel value
00280     img_ptr = (float*)&image->data[0];
00281     float minValue = std::numeric_limits<float>::max();
00282     float maxValue = std::numeric_limits<float>::min();
00283 
00284     for( i = 0; i < img_size; ++i )
00285     {
00286       minValue = std::min( minValue, *img_ptr );
00287       maxValue = std::max( maxValue, *img_ptr );
00288       img_ptr++;
00289     }
00290 
00291     // Rescale floating-point image
00292     float dynamic_range = maxValue - minValue;
00293     if( dynamic_range > 0.0f )
00294     {
00295       img_ptr = (float*) &image->data[0];
00296       for( i = 0; i < img_size; ++i )
00297       {
00298         *img_ptr -= minValue;
00299         *img_ptr /= dynamic_range;
00300         img_ptr++;
00301       }
00302     }
00303   }
00304   else
00305   {
00306     throw UnsupportedImageEncoding(image->encoding);
00307   }
00308 
00309   width_ = image->width;
00310   height_ = image->height;
00311 
00312   // TODO: Support different steps/strides
00313 
00314   Ogre::DataStreamPtr pixel_stream;
00315   pixel_stream.bind(new Ogre::MemoryDataStream(data_ptr, data_size));
00316 
00317   try
00318   {
00319     ogre_image.loadRawData(pixel_stream, width_, height_, 1, format, 1, 0);
00320   }
00321   catch (Ogre::Exception& e)
00322   {
00323     // TODO: signal error better
00324     ROS_ERROR("Error loading image: %s", e.what());
00325     return false;
00326   }
00327 
00328   texture_->unload();
00329   texture_->loadImage(ogre_image);
00330 
00331   return true;
00332 }
00333 
00334 void ROSImageTexture::callback(const sensor_msgs::Image::ConstPtr& msg)
00335 {
00336   boost::mutex::scoped_lock lock(mutex_);
00337   current_image_ = msg;
00338   new_image_ = true;
00339 
00340   ++image_count_;
00341 }
00342 
00343 void ROSImageTexture::scanForTransportSubscriberPlugins()
00344 {
00345   pluginlib::ClassLoader<image_transport::SubscriberPlugin> sub_loader( "image_transport", "image_transport::SubscriberPlugin" );
00346 
00347   BOOST_FOREACH( const std::string& lookup_name, sub_loader.getDeclaredClasses() )
00348   {
00349     // lookup_name is formatted as "pkg/transport_sub", for instance
00350     // "image_transport/compressed_sub" for the "compressed"
00351     // transport.  This code removes the "_sub" from the tail and
00352     // everything up to and including the "/" from the head, leaving
00353     // "compressed" (for example) in transport_name.
00354     std::string transport_name = boost::erase_last_copy( lookup_name, "_sub" );
00355     transport_name = transport_name.substr( lookup_name.find( '/' ) + 1 );
00356 
00357     // If the plugin loads without throwing an exception, add its
00358     // transport name to the list of valid plugins, otherwise ignore
00359     // it.
00360     try
00361     {
00362       boost::shared_ptr<image_transport::SubscriberPlugin> sub = sub_loader.createInstance( lookup_name );
00363       transport_plugin_types_.insert( transport_name );
00364     }
00365     catch( const pluginlib::LibraryLoadException& e ) {}
00366     catch( const pluginlib::CreateClassException& e ) {}
00367   }
00368 }
00369 
00370 } // end of namespace rviz


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32