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 "ros_image_texture.h"
00031 #include "sensor_msgs/image_encodings.h"
00032 #include "rviz/uniform_string_stream.h"
00033
00034 #include <tf/tf.h>
00035
00036 #include <OGRE/OgreTextureManager.h>
00037
00038 namespace rviz
00039 {
00040
00041 ROSImageTexture::ROSImageTexture(const ros::NodeHandle& nh)
00042 : nh_(nh)
00043 , it_(nh_)
00044 , transport_type_("raw")
00045 , new_image_(false)
00046 , width_(0)
00047 , height_(0)
00048 , tf_client_(0)
00049 , image_count_(0)
00050 , queue_size_(2)
00051 {
00052 empty_image_.load("no_image.png", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00053
00054 static uint32_t count = 0;
00055 UniformStringStream ss;
00056 ss << "ROSImageTexture" << count++;
00057 texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, empty_image_, Ogre::TEX_TYPE_2D, 0);
00058 }
00059
00060 ROSImageTexture::~ROSImageTexture()
00061 {
00062 current_image_.reset();
00063 }
00064
00065 void ROSImageTexture::clear()
00066 {
00067 boost::mutex::scoped_lock lock(mutex_);
00068
00069 texture_->unload();
00070 texture_->loadImage(empty_image_);
00071
00072 new_image_ = false;
00073 current_image_.reset();
00074
00075 if (tf_filter_)
00076 {
00077 tf_filter_->clear();
00078 }
00079
00080 image_count_ = 0;
00081 }
00082
00083 void ROSImageTexture::setFrame(const std::string& frame, tf::TransformListener* tf_client)
00084 {
00085 tf_client_ = tf_client;
00086 frame_ = frame;
00087 setTopic(topic_);
00088 }
00089
00090 void ROSImageTexture::setTopic(const std::string& topic)
00091 {
00092 boost::mutex::scoped_lock lock(mutex_);
00093
00094
00095 current_image_.reset();
00096
00097 topic_ = topic;
00098 tf_filter_.reset();
00099
00100 if (!sub_)
00101 {
00102 sub_.reset(new image_transport::SubscriberFilter());
00103 }
00104
00105 if (!topic.empty())
00106 {
00107 sub_->subscribe(it_, topic, 1, image_transport::TransportHints(transport_type_));
00108
00109 if (frame_.empty())
00110 {
00111 sub_->registerCallback(boost::bind(&ROSImageTexture::callback, this, _1));
00112 }
00113 else
00114 {
00115 ROS_ASSERT(tf_client_);
00116 tf_filter_.reset(new tf::MessageFilter<sensor_msgs::Image>(*sub_, (tf::Transformer&)*tf_client_, frame_, queue_size_, nh_));
00117 tf_filter_->registerCallback(boost::bind(&ROSImageTexture::callback, this, _1));
00118 }
00119 }
00120 }
00121
00122 void ROSImageTexture::setTransportType(const std::string& transport_type)
00123 {
00124 transport_type_ = transport_type;
00125 setTopic(topic_);
00126 }
00127
00128 void ROSImageTexture::getAvailableTransportTypes(V_string& types)
00129 {
00130 types.push_back("raw");
00131
00132 ros::master::V_TopicInfo topics;
00133 ros::master::getTopics(topics);
00134 ros::master::V_TopicInfo::iterator it = topics.begin();
00135 ros::master::V_TopicInfo::iterator end = topics.end();
00136 for (; it != end; ++it)
00137 {
00138 const ros::master::TopicInfo& ti = *it;
00139 if (ti.name.find(topic_) == 0 && ti.name != topic_)
00140 {
00141 std::string type = ti.name.substr(topic_.size() + 1);
00142 if (type.find('/') == std::string::npos)
00143 {
00144 types.push_back(type);
00145 }
00146 }
00147 }
00148 }
00149
00150 const sensor_msgs::Image::ConstPtr& ROSImageTexture::getImage()
00151 {
00152 boost::mutex::scoped_lock lock(mutex_);
00153
00154 return current_image_;
00155 }
00156
00157 void ROSImageTexture::setQueueSize( int size )
00158 {
00159 queue_size_ = size;
00160 if( tf_filter_ )
00161 {
00162 tf_filter_->setQueueSize( (uint32_t) size );
00163 }
00164 }
00165
00166 int ROSImageTexture::getQueueSize()
00167 {
00168 return queue_size_;
00169 }
00170
00171 bool ROSImageTexture::update()
00172 {
00173 sensor_msgs::Image::ConstPtr image;
00174 bool new_image = false;
00175 {
00176 boost::mutex::scoped_lock lock(mutex_);
00177
00178 image = current_image_;
00179 new_image = new_image_;
00180 }
00181
00182 if (!image || !new_image)
00183 {
00184 return false;
00185 }
00186
00187 new_image_ = false;
00188
00189 if (image->data.empty())
00190 {
00191 return false;
00192 }
00193
00194 Ogre::PixelFormat format = Ogre::PF_R8G8B8;
00195 Ogre::Image ogre_image;
00196 std::vector<uint8_t> buffer;
00197 void* data_ptr = (void*)&image->data[0];
00198 uint32_t data_size = image->data.size();
00199 if (image->encoding == sensor_msgs::image_encodings::RGB8)
00200 {
00201 format = Ogre::PF_BYTE_RGB;
00202 }
00203 else if (image->encoding == sensor_msgs::image_encodings::RGBA8)
00204 {
00205 format = Ogre::PF_BYTE_RGBA;
00206 }
00207 else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC4 ||
00208 image->encoding == sensor_msgs::image_encodings::TYPE_8SC4 ||
00209 image->encoding == sensor_msgs::image_encodings::BGRA8)
00210 {
00211 format = Ogre::PF_BYTE_BGRA;
00212 }
00213 else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC3 ||
00214 image->encoding == sensor_msgs::image_encodings::TYPE_8SC3 ||
00215 image->encoding == sensor_msgs::image_encodings::BGR8)
00216 {
00217 format = Ogre::PF_BYTE_BGR;
00218 }
00219 else if (image->encoding == sensor_msgs::image_encodings::TYPE_8UC1 ||
00220 image->encoding == sensor_msgs::image_encodings::TYPE_8SC1 ||
00221 image->encoding == sensor_msgs::image_encodings::MONO8)
00222 {
00223 format = Ogre::PF_BYTE_L;
00224 }
00225 else if (image->encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
00226 image->encoding == sensor_msgs::image_encodings::TYPE_16SC1 ||
00227 image->encoding == sensor_msgs::image_encodings::MONO16)
00228 {
00229 format = Ogre::PF_BYTE_L;
00230
00231
00232 buffer.resize(image->data.size() / 2);
00233 data_size = buffer.size();
00234 data_ptr = (void*)&buffer[0];
00235 for (size_t i = 0; i < data_size; ++i)
00236 {
00237 uint16_t s = image->data[2*i] << 8 | image->data[2*i + 1];
00238 float val = (float)s / std::numeric_limits<uint16_t>::max();
00239 buffer[i] = val * 255;
00240 }
00241 }
00242 else if (image->encoding.find("bayer") == 0)
00243 {
00244 format = Ogre::PF_BYTE_L;
00245 }
00246 else
00247 {
00248 throw UnsupportedImageEncoding(image->encoding);
00249 }
00250
00251 width_ = image->width;
00252 height_ = image->height;
00253
00254
00255
00256 Ogre::DataStreamPtr pixel_stream;
00257 pixel_stream.bind(new Ogre::MemoryDataStream(data_ptr, data_size));
00258
00259 try
00260 {
00261 ogre_image.loadRawData(pixel_stream, width_, height_, 1, format, 1, 0);
00262 }
00263 catch (Ogre::Exception& e)
00264 {
00265
00266 ROS_ERROR("Error loading image: %s", e.what());
00267 return false;
00268 }
00269
00270 texture_->unload();
00271 texture_->loadImage(ogre_image);
00272
00273 return true;
00274 }
00275
00276 void ROSImageTexture::callback(const sensor_msgs::Image::ConstPtr& msg)
00277 {
00278 boost::mutex::scoped_lock lock(mutex_);
00279 current_image_ = msg;
00280 new_image_ = true;
00281
00282 ++image_count_;
00283 }
00284
00285 }