$search
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 #ifndef RVIZ_ROS_IMAGE_TEXTURE_H 00031 #define RVIZ_ROS_IMAGE_TEXTURE_H 00032 00033 #include <sensor_msgs/Image.h> 00034 00035 #include <OGRE/OgreTexture.h> 00036 #include <OGRE/OgreImage.h> 00037 00038 #include <boost/shared_ptr.hpp> 00039 #include <boost/thread/mutex.hpp> 00040 #include <cv_bridge/cv_bridge.h> 00041 #include <ros/ros.h> 00042 #include <image_transport/image_transport.h> 00043 #include <image_transport/subscriber_filter.h> 00044 00045 #include <tf/message_filter.h> 00046 00047 #include <stdexcept> 00048 00049 namespace tf 00050 { 00051 class TransformListener; 00052 } 00053 00054 00055 namespace srs_ui_but 00056 { 00057 00058 00062 class CRosTextureConverter 00063 { 00064 public: 00068 class RTCException : public std::runtime_error 00069 { 00070 public: 00072 RTCException(const std::string& estr) 00073 : std::runtime_error(estr) 00074 {} 00075 00076 }; // class RTCException 00077 00078 public: 00080 CRosTextureConverter( const std::string & encoding = std::string(), bool bStaticTexture = false ); 00081 00083 CRosTextureConverter( const std::string & name, const std::string & encoding, bool bStaticTexture = false ); 00084 00086 ~CRosTextureConverter(); 00087 00089 void setEncoding( const std::string & encoding ){ m_output_encoding = encoding; } 00090 00092 bool convert( const sensor_msgs::Image::ConstPtr& image, const std::string & texture_name, bool writeInfo = false ); 00093 00095 bool convert( const sensor_msgs::Image::ConstPtr& image, bool writeInfo = false ) 00096 { 00097 return convert( image, m_texture_name, writeInfo ); 00098 } 00099 00101 Ogre::TexturePtr & getOgreTexture(){ return m_ogre_image; } 00102 00104 cv_bridge::CvImage * getOpenCVImage() { return m_cv_image.get(); } 00105 00106 00107 00108 protected: 00110 void initOgreTexture( Ogre::PixelFormat format, const std::string & name ); 00111 00113 Ogre::PixelFormat getFormat( const std::string & encoding ); 00114 00116 void writeStats( Ogre::Image & image ); 00117 00118 protected: 00120 cv_bridge::CvImagePtr m_cv_image; 00121 00123 Ogre::TexturePtr m_ogre_image; 00124 00126 std::string m_output_encoding; 00127 00129 Ogre::PixelFormat m_static_format; 00130 00132 std::string m_texture_name; 00133 00135 Ogre::Image m_empty_image; 00136 00138 bool m_bStaticTexture; 00139 00140 00141 }; // class CRosTextureConverter 00142 00143 class CRosTopicTexture 00144 { 00145 public: 00147 CRosTopicTexture( const ros::NodeHandle& nh, const std::string & texture_name, const std::string & encoding ); 00148 00150 //CRosTopicTexture( const ros::NodeHandle& nh, const std::string & texture_name, Ogre::PixelFormat encoding ); 00151 00153 ~CRosTopicTexture(); 00154 00156 void setTopic(const std::string& topic); 00157 00159 std::string getTopic() { return m_topic; } 00160 00162 const Ogre::TexturePtr& getTexture() { return m_texture_converter.getOgreTexture(); } 00163 00165 bool update(); 00166 00168 void clear(); 00169 00170 protected: 00172 void callback(const sensor_msgs::Image::ConstPtr& image); 00173 00174 protected: 00176 ros::NodeHandle m_nh; 00177 00179 CRosTextureConverter m_texture_converter; 00180 00182 std::string m_name; 00183 00185 std::string m_topic; 00186 00187 sensor_msgs::Image::ConstPtr m_current_image; 00188 image_transport::ImageTransport m_it; 00189 boost::shared_ptr<image_transport::SubscriberFilter> m_sub; 00190 boost::shared_ptr<tf::MessageFilter<sensor_msgs::Image> > m_tf_filter; 00191 std::string m_transport_type; 00192 boost::mutex m_mutex; 00193 bool m_new_image; 00194 std::string m_frame; 00195 tf::TransformListener* m_tf_client; 00196 00197 }; 00198 00199 00200 00201 } // namespace srs_ui_but 00202 00203 00204 #endif