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 <OGRE/OgreManualObject.h>
00033 #include <OGRE/OgreMaterialManager.h>
00034 #include <OGRE/OgreRectangle2D.h>
00035 #include <OGRE/OgreRenderSystem.h>
00036 #include <OGRE/OgreRenderWindow.h>
00037 #include <OGRE/OgreRoot.h>
00038 #include <OGRE/OgreSceneManager.h>
00039 #include <OGRE/OgreSceneNode.h>
00040 #include <OGRE/OgreTextureManager.h>
00041 #include <OGRE/OgreViewport.h>
00042
00043 #include <tf/transform_listener.h>
00044
00045 #include "rviz/display_context.h"
00046 #include "rviz/frame_manager.h"
00047 #include "rviz/panel_dock_widget.h"
00048 #include "rviz/properties/editable_enum_property.h"
00049 #include "rviz/properties/int_property.h"
00050 #include "rviz/properties/ros_topic_property.h"
00051 #include "rviz/render_panel.h"
00052 #include "rviz/validate_floats.h"
00053 #include "rviz/window_manager_interface.h"
00054
00055 #include "image_display.h"
00056
00057 namespace rviz
00058 {
00059
00060 ImageDisplay::ImageDisplay()
00061 : Display()
00062 , transport_("raw")
00063 , texture_(update_nh_)
00064 , panel_container_( 0 )
00065 {
00066 topic_property_ = new RosTopicProperty( "Image Topic", "",
00067 QString::fromStdString( ros::message_traits::datatype<sensor_msgs::Image>() ),
00068 "sensor_msgs::Image topic to subscribe to.",
00069 this, SLOT( updateTopic() ));
00070
00071 transport_property_ = new EditableEnumProperty( "Transport Hint", "raw",
00072 "Preferred method of sending images.",
00073 this, SLOT( updateTransport() ));
00074 connect( transport_property_, SIGNAL( requestOptions( QStringList* )),
00075 this, SLOT( fillTransportOptionList( QStringList* )));
00076 }
00077
00078 void ImageDisplay::onInitialize()
00079 {
00080 {
00081 static uint32_t count = 0;
00082 std::stringstream ss;
00083 ss << "ImageDisplay" << count++;
00084 scene_manager_ = Ogre::Root::getSingleton().createSceneManager(Ogre::ST_GENERIC, ss.str());
00085 }
00086
00087 scene_node_ = 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 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(scene_manager_, context_);
00124
00125 WindowManagerInterface* wm = context_->getWindowManager();
00126 if (wm)
00127 {
00128 panel_container_ = wm->addPane( getName().toStdString(), render_panel_);
00129 }
00130 render_panel_->setAutoRender(false);
00131 render_panel_->setOverlaysEnabled(false);
00132 render_panel_->getCamera()->setNearClipDistance( 0.01f );
00133
00134 if( panel_container_ )
00135 {
00136 connect( panel_container_, SIGNAL( visibilityChanged( bool ) ), this, SLOT( setEnabled( bool )));
00137 }
00138 }
00139
00140 ImageDisplay::~ImageDisplay()
00141 {
00142 unsubscribe();
00143
00144 if( render_panel_ )
00145 {
00146 if( panel_container_ )
00147 {
00148 delete panel_container_;
00149 }
00150 else
00151 {
00152 delete render_panel_;
00153 }
00154 }
00155
00156 delete screen_rect_;
00157
00158 scene_node_->getParentSceneNode()->removeAndDestroyChild(scene_node_->getName());
00159 }
00160
00161 void ImageDisplay::onEnable()
00162 {
00163 subscribe();
00164
00165 if( render_panel_->parentWidget() == 0 )
00166 {
00167 render_panel_->show();
00168 }
00169 else
00170 {
00171 panel_container_->show();
00172 }
00173
00174 render_panel_->getRenderWindow()->setActive(true);
00175 }
00176
00177 void ImageDisplay::onDisable()
00178 {
00179 render_panel_->getRenderWindow()->setActive(false);
00180
00181 if( render_panel_->parentWidget() == 0 )
00182 {
00183 if( render_panel_->isVisible() )
00184 {
00185 render_panel_->hide();
00186 }
00187 }
00188 else
00189 {
00190 if( panel_container_->isVisible() )
00191 {
00192 panel_container_->close();
00193 }
00194 }
00195
00196 unsubscribe();
00197
00198 clear();
00199 }
00200
00201 void ImageDisplay::subscribe()
00202 {
00203 if ( !isEnabled() )
00204 {
00205 return;
00206 }
00207
00208 try
00209 {
00210 texture_.setTopic( topic_property_->getTopicStd() );
00211 setStatus( StatusProperty::Ok, "Topic", "OK" );
00212 }
00213 catch( ros::Exception& e )
00214 {
00215 setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() );
00216 }
00217 }
00218
00219 void ImageDisplay::unsubscribe()
00220 {
00221 texture_.setTopic("");
00222 }
00223
00224 void ImageDisplay::updateTopic()
00225 {
00226 unsubscribe();
00227 clear();
00228 subscribe();
00229 }
00230
00231 void ImageDisplay::updateTransport()
00232 {
00233 texture_.setTransportType( transport_property_->getStdString() );
00234 }
00235
00236 void ImageDisplay::clear()
00237 {
00238 texture_.clear();
00239
00240 setStatus(StatusProperty::Warn, "Image", "No Image received");
00241
00242 if( render_panel_->getCamera() )
00243 {
00244 render_panel_->getCamera()->setPosition(Ogre::Vector3(999999, 999999, 999999));
00245 }
00246 }
00247
00248 void ImageDisplay::updateStatus()
00249 {
00250 if( texture_.getImageCount() == 0 )
00251 {
00252 setStatus( StatusProperty::Warn, "Image", "No image received" );
00253 }
00254 else
00255 {
00256 setStatus( StatusProperty::Ok, "Image", QString::number( texture_.getImageCount() ) + " images received" );
00257 }
00258 }
00259
00260 void ImageDisplay::update( float wall_dt, float ros_dt )
00261 {
00262 updateStatus();
00263
00264 try
00265 {
00266 texture_.update();
00267
00268
00269 float win_width = render_panel_->width();
00270 float win_height = render_panel_->height();
00271
00272 float img_width = texture_.getWidth();
00273 float img_height = texture_.getHeight();
00274
00275 if ( img_width != 0 && img_height != 0 && win_width !=0 && win_height != 0 )
00276 {
00277 float img_aspect = img_width / img_height;
00278 float win_aspect = win_width / win_height;
00279
00280 if ( img_aspect > win_aspect )
00281 {
00282 screen_rect_->setCorners(-1.0f, 1.0f * win_aspect/img_aspect, 1.0f, -1.0f * win_aspect/img_aspect, false);
00283 }
00284 else
00285 {
00286 screen_rect_->setCorners(-1.0f * img_aspect/win_aspect, 1.0f, 1.0f * img_aspect/win_aspect, -1.0f, false);
00287 }
00288 }
00289
00290 render_panel_->getRenderWindow()->update();
00291 }
00292 catch( UnsupportedImageEncoding& e )
00293 {
00294 setStatus(StatusProperty::Error, "Image", e.what());
00295 }
00296 }
00297
00298 void ImageDisplay::fillTransportOptionList( QStringList* qchoices_out )
00299 {
00300 V_string choices;
00301 texture_.getAvailableTransportTypes( choices );
00302 for( size_t i = 0; i < choices.size(); i++ )
00303 {
00304 qchoices_out->append( QString::fromStdString( choices[ i ]));
00305 }
00306 }
00307
00308 void ImageDisplay::reset()
00309 {
00310 Display::reset();
00311 clear();
00312 }
00313
00314 void ImageDisplay::setName( const QString& name )
00315 {
00316 Display::setName( name );
00317 if( panel_container_ )
00318 {
00319 panel_container_->setWindowTitle( name );
00320 panel_container_->setObjectName( name );
00321 }
00322 else
00323 {
00324 render_panel_->setWindowTitle( name );
00325 }
00326 }
00327
00328 }
00329
00330 #include <pluginlib/class_list_macros.h>
00331 PLUGINLIB_DECLARE_CLASS( rviz, Image, rviz::ImageDisplay, rviz::Display )