Go to the documentation of this file.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 <boost/bind.hpp>
00031
00032 #include <OgreManualObject.h>
00033 #include <OgreMaterialManager.h>
00034 #include <OgreRectangle2D.h>
00035 #include <OgreRenderSystem.h>
00036 #include <OgreRenderWindow.h>
00037 #include <OgreRoot.h>
00038 #include <OgreSceneManager.h>
00039 #include <OgreSceneNode.h>
00040 #include <OgreTextureManager.h>
00041 #include <OgreViewport.h>
00042 #include <OgreTechnique.h>
00043 #include <OgreCamera.h>
00044
00045 #include <tf/transform_listener.h>
00046
00047 #include "rviz/display_context.h"
00048 #include "rviz/frame_manager.h"
00049 #include "rviz/render_panel.h"
00050 #include "rviz/validate_floats.h"
00051
00052 #include <sensor_msgs/image_encodings.h>
00053
00054 #include "image_display.h"
00055
00056 namespace rviz
00057 {
00058
00059 ImageDisplay::ImageDisplay()
00060 : ImageDisplayBase()
00061 , texture_()
00062 {
00063 normalize_property_ = new BoolProperty( "Normalize Range", true,
00064 "If set to true, will try to estimate the range of possible values from the received images.",
00065 this, SLOT( updateNormalizeOptions() ));
00066
00067 min_property_ = new FloatProperty( "Min Value", 0.0, "Value which will be displayed as black.", this, SLOT( updateNormalizeOptions() ));
00068
00069 max_property_ = new FloatProperty( "Max Value", 1.0, "Value which will be displayed as white.", this, SLOT( updateNormalizeOptions() ));
00070
00071 median_buffer_size_property_ = new IntProperty( "Median window", 5, "Window size for median filter used for computin min/max.",
00072 this, SLOT( updateNormalizeOptions() ) );
00073
00074 got_float_image_ = false;
00075 }
00076
00077 void ImageDisplay::onInitialize()
00078 {
00079 ImageDisplayBase::onInitialize();
00080 {
00081 static uint32_t count = 0;
00082 std::stringstream ss;
00083 ss << "ImageDisplay" << count++;
00084 img_scene_manager_ = Ogre::Root::getSingleton().createSceneManager(Ogre::ST_GENERIC, ss.str());
00085 }
00086
00087 img_scene_node_ = img_scene_manager_->getRootSceneNode()->createChildSceneNode();
00088
00089 {
00090 static int count = 0;
00091 std::stringstream ss;
00092 ss << "ImageDisplayObject" << count++;
00093
00094 screen_rect_ = new Ogre::Rectangle2D(true);
00095 screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1);
00096 screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00097
00098 ss << "Material";
00099 material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
00100 material_->setSceneBlending( Ogre::SBT_REPLACE );
00101 material_->setDepthWriteEnabled(false);
00102 material_->setReceiveShadows(false);
00103 material_->setDepthCheckEnabled(false);
00104
00105 material_->getTechnique(0)->setLightingEnabled(false);
00106 Ogre::TextureUnitState* tu = material_->getTechnique(0)->getPass(0)->createTextureUnitState();
00107 tu->setTextureName(texture_.getTexture()->getName());
00108 tu->setTextureFiltering( Ogre::TFO_NONE );
00109
00110 material_->setCullingMode(Ogre::CULL_NONE);
00111 Ogre::AxisAlignedBox aabInf;
00112 aabInf.setInfinite();
00113 screen_rect_->setBoundingBox(aabInf);
00114 screen_rect_->setMaterial(material_->getName());
00115 img_scene_node_->attachObject(screen_rect_);
00116 }
00117
00118 render_panel_ = new RenderPanel();
00119 render_panel_->getRenderWindow()->setAutoUpdated(false);
00120 render_panel_->getRenderWindow()->setActive( false );
00121
00122 render_panel_->resize( 640, 480 );
00123 render_panel_->initialize(img_scene_manager_, context_);
00124
00125 setAssociatedWidget( render_panel_ );
00126
00127 render_panel_->setAutoRender(false);
00128 render_panel_->setOverlaysEnabled(false);
00129 render_panel_->getCamera()->setNearClipDistance( 0.01f );
00130
00131 updateNormalizeOptions();
00132 }
00133
00134 ImageDisplay::~ImageDisplay()
00135 {
00136 if ( initialized() )
00137 {
00138 delete render_panel_;
00139 delete screen_rect_;
00140 img_scene_node_->getParentSceneNode()->removeAndDestroyChild( img_scene_node_->getName() );
00141 }
00142 }
00143
00144 void ImageDisplay::onEnable()
00145 {
00146 ImageDisplayBase::subscribe();
00147 render_panel_->getRenderWindow()->setActive(true);
00148 }
00149
00150 void ImageDisplay::onDisable()
00151 {
00152 render_panel_->getRenderWindow()->setActive(false);
00153 ImageDisplayBase::unsubscribe();
00154 clear();
00155 }
00156
00157 void ImageDisplay::updateNormalizeOptions()
00158 {
00159 if (got_float_image_)
00160 {
00161 bool normalize = normalize_property_->getBool();
00162
00163 normalize_property_->setHidden(false);
00164 min_property_->setHidden(normalize);
00165 max_property_->setHidden(normalize);
00166 median_buffer_size_property_->setHidden(!normalize);
00167
00168 texture_.setNormalizeFloatImage( normalize, min_property_->getFloat(), max_property_->getFloat());
00169 texture_.setMedianFrames( median_buffer_size_property_->getInt() );
00170 }
00171 else
00172 {
00173 normalize_property_->setHidden(true);
00174 min_property_->setHidden(true);
00175 max_property_->setHidden(true);
00176 median_buffer_size_property_->setHidden(true);
00177 }
00178 }
00179
00180 void ImageDisplay::clear()
00181 {
00182 texture_.clear();
00183
00184 if( render_panel_->getCamera() )
00185 {
00186 render_panel_->getCamera()->setPosition(Ogre::Vector3(999999, 999999, 999999));
00187 }
00188 }
00189
00190 void ImageDisplay::update( float wall_dt, float ros_dt )
00191 {
00192 try
00193 {
00194 texture_.update();
00195
00196
00197 float win_width = render_panel_->width();
00198 float win_height = render_panel_->height();
00199
00200 float img_width = texture_.getWidth();
00201 float img_height = texture_.getHeight();
00202
00203 if ( img_width != 0 && img_height != 0 && win_width !=0 && win_height != 0 )
00204 {
00205 float img_aspect = img_width / img_height;
00206 float win_aspect = win_width / win_height;
00207
00208 if ( img_aspect > win_aspect )
00209 {
00210 screen_rect_->setCorners(-1.0f, 1.0f * win_aspect/img_aspect, 1.0f, -1.0f * win_aspect/img_aspect, false);
00211 }
00212 else
00213 {
00214 screen_rect_->setCorners(-1.0f * img_aspect/win_aspect, 1.0f, 1.0f * img_aspect/win_aspect, -1.0f, false);
00215 }
00216 }
00217
00218 render_panel_->getRenderWindow()->update();
00219 }
00220 catch( UnsupportedImageEncoding& e )
00221 {
00222 setStatus(StatusProperty::Error, "Image", e.what());
00223 }
00224 }
00225
00226 void ImageDisplay::reset()
00227 {
00228 ImageDisplayBase::reset();
00229 clear();
00230 }
00231
00232
00233 void ImageDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg)
00234 {
00235 bool got_float_image = msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1 ||
00236 msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
00237 msg->encoding == sensor_msgs::image_encodings::TYPE_16SC1 ||
00238 msg->encoding == sensor_msgs::image_encodings::MONO16;
00239
00240 if ( got_float_image != got_float_image_ )
00241 {
00242 got_float_image_ = got_float_image;
00243 updateNormalizeOptions();
00244 }
00245 texture_.addMessage(msg);
00246 }
00247
00248 }
00249
00250 #include <pluginlib/class_list_macros.h>
00251 PLUGINLIB_EXPORT_CLASS( rviz::ImageDisplay, rviz::Display )