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 "image_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/render_panel.h"
00033 #include "rviz/properties/property.h"
00034 #include "rviz/properties/property_manager.h"
00035 #include "rviz/window_manager_interface.h"
00036 #include "rviz/frame_manager.h"
00037 #include "rviz/validate_floats.h"
00038 #include "rviz/panel_dock_widget.h"
00039 #include "rviz/display_wrapper.h"
00040
00041 #include <tf/transform_listener.h>
00042
00043 #include <boost/bind.hpp>
00044
00045 #include <ogre_tools/render_system.h>
00046
00047 #include <OGRE/OgreSceneNode.h>
00048 #include <OGRE/OgreSceneManager.h>
00049 #include <OGRE/OgreRectangle2D.h>
00050 #include <OGRE/OgreMaterialManager.h>
00051 #include <OGRE/OgreTextureManager.h>
00052 #include <OGRE/OgreViewport.h>
00053 #include <OGRE/OgreRenderWindow.h>
00054 #include <OGRE/OgreManualObject.h>
00055 #include <OGRE/OgreRoot.h>
00056 #include <OGRE/OgreRenderSystem.h>
00057
00058 namespace rviz
00059 {
00060
00061 ImageDisplay::Panel::Panel( ImageDisplay* display, QWidget* parent )
00062 : RenderPanel( ogre_tools::RenderSystem::get(), display, parent )
00063 , display_( display )
00064 {
00065 }
00066
00067 void ImageDisplay::Panel::showEvent( QShowEvent* event )
00068 {
00069 RenderPanel::showEvent( event );
00070 render_window_->setAutoUpdated(false);
00071 render_window_->setActive( active_ );
00072 display_->setTopic( display_->getTopic() );
00073 }
00074
00075 void ImageDisplay::Panel::setActive( bool active )
00076 {
00077 active_ = active;
00078 if( render_window_ != 0 )
00079 {
00080 render_window_->setActive( active_ );
00081 }
00082 }
00083
00084 void ImageDisplay::Panel::updateRenderWindow()
00085 {
00086 if( render_window_ != 0 )
00087 {
00088 render_window_->update();
00089 }
00090 }
00091
00092 ImageDisplay::ImageDisplay()
00093 : Display()
00094 , transport_("raw")
00095 , texture_(update_nh_)
00096 , panel_container_( 0 )
00097 {
00098 }
00099
00100 void ImageDisplay::onInitialize()
00101 {
00102 {
00103 static uint32_t count = 0;
00104 std::stringstream ss;
00105 ss << "ImageDisplay" << count++;
00106 scene_manager_ = Ogre::Root::getSingleton().createSceneManager(Ogre::ST_GENERIC, ss.str());
00107 }
00108
00109 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00110
00111 {
00112 static int count = 0;
00113 std::stringstream ss;
00114 ss << "ImageDisplayObject" << count++;
00115
00116 screen_rect_ = new Ogre::Rectangle2D(true);
00117 screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1);
00118 screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00119
00120 ss << "Material";
00121 material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
00122 material_->setSceneBlending( Ogre::SBT_REPLACE );
00123 material_->setDepthWriteEnabled(false);
00124 material_->setReceiveShadows(false);
00125 material_->setDepthCheckEnabled(false);
00126
00127 material_->getTechnique(0)->setLightingEnabled(false);
00128 Ogre::TextureUnitState* tu = material_->getTechnique(0)->getPass(0)->createTextureUnitState();
00129 tu->setTextureName(texture_.getTexture()->getName());
00130 tu->setTextureFiltering( Ogre::TFO_NONE );
00131
00132 material_->setCullingMode(Ogre::CULL_NONE);
00133 Ogre::AxisAlignedBox aabInf;
00134 aabInf.setInfinite();
00135 screen_rect_->setBoundingBox(aabInf);
00136 screen_rect_->setMaterial(material_->getName());
00137 scene_node_->attachObject(screen_rect_);
00138
00139 }
00140
00141 QWidget* parent = 0;
00142
00143 WindowManagerInterface* wm = vis_manager_->getWindowManager();
00144 if (wm)
00145 {
00146 parent = wm->getParentWindow();
00147 }
00148
00149 render_panel_ = new Panel( this, parent );
00150 render_panel_->resize( 640, 480 );
00151 render_panel_->initialize(scene_manager_, vis_manager_);
00152 if (wm)
00153 {
00154 panel_container_ = wm->addPane(name_, render_panel_);
00155 panel_container_->hide();
00156 }
00157 render_panel_->setAutoRender(false);
00158 render_panel_->setOverlaysEnabled(false);
00159 render_panel_->getCamera()->setNearClipDistance( 0.01f );
00160
00161 if( panel_container_ )
00162 {
00163 connect( panel_container_, SIGNAL( visibilityChanged( bool ) ), this, SLOT( setWrapperEnabled( bool )));
00164 }
00165 }
00166
00167 ImageDisplay::~ImageDisplay()
00168 {
00169 unsubscribe();
00170
00171 if( render_panel_ )
00172 {
00173 if( panel_container_ )
00174 {
00175 delete panel_container_;
00176 }
00177 else
00178 {
00179 delete render_panel_;
00180 }
00181 }
00182
00183 delete screen_rect_;
00184
00185 scene_node_->getParentSceneNode()->removeAndDestroyChild(scene_node_->getName());
00186 }
00187
00188 void ImageDisplay::setWrapperEnabled( bool enabled )
00189 {
00190
00191
00192 DisplayWrapper* wrapper = vis_manager_->getDisplayWrapper( this );
00193 if( wrapper != NULL )
00194 {
00195 wrapper->setEnabled( enabled );
00196 }
00197 }
00198
00199 void ImageDisplay::onEnable()
00200 {
00201 subscribe();
00202
00203 if( render_panel_->parentWidget() == 0 )
00204 {
00205 render_panel_->show();
00206 }
00207 else
00208 {
00209 panel_container_->show();
00210 }
00211
00212 render_panel_->setActive(true);
00213 }
00214
00215 void ImageDisplay::onDisable()
00216 {
00217 render_panel_->setActive(false);
00218
00219 if( render_panel_->parentWidget() == 0 )
00220 {
00221 if( render_panel_->isVisible() )
00222 {
00223 render_panel_->hide();
00224 }
00225 }
00226 else
00227 {
00228 if( panel_container_->isVisible() )
00229 {
00230 panel_container_->close();
00231 }
00232 }
00233
00234 unsubscribe();
00235
00236 clear();
00237 }
00238
00239 void ImageDisplay::subscribe()
00240 {
00241 if ( !isEnabled() )
00242 {
00243 return;
00244 }
00245
00246 texture_.setTopic(topic_);
00247 }
00248
00249 void ImageDisplay::unsubscribe()
00250 {
00251 texture_.setTopic("");
00252 }
00253
00254 void ImageDisplay::setTopic( const std::string& topic )
00255 {
00256 unsubscribe();
00257
00258 topic_ = topic;
00259 clear();
00260
00261 subscribe();
00262
00263 propertyChanged(topic_property_);
00264 }
00265
00266 void ImageDisplay::setTransport(const std::string& transport)
00267 {
00268 transport_ = transport;
00269
00270 texture_.setTransportType(transport);
00271
00272 propertyChanged(transport_property_);
00273 }
00274
00275 void ImageDisplay::clear()
00276 {
00277 texture_.clear();
00278
00279 setStatus(status_levels::Warn, "Image", "No Image received");
00280
00281 if( render_panel_->getCamera() )
00282 {
00283 render_panel_->getCamera()->setPosition(Ogre::Vector3(999999, 999999, 999999));
00284 }
00285 }
00286
00287 void ImageDisplay::updateStatus()
00288 {
00289 if (texture_.getImageCount() == 0)
00290 {
00291 setStatus(status_levels::Warn, "Image", "No image received");
00292 }
00293 else
00294 {
00295 std::stringstream ss;
00296 ss << texture_.getImageCount() << " images received";
00297 setStatus(status_levels::Ok, "Image", ss.str());
00298 }
00299 }
00300
00301 void ImageDisplay::update(float wall_dt, float ros_dt)
00302 {
00303 updateStatus();
00304
00305 try
00306 {
00307 texture_.update();
00308
00309
00310 float win_width = render_panel_->width();
00311 float win_height = render_panel_->height();
00312
00313 float img_width = texture_.getWidth();
00314 float img_height = texture_.getHeight();
00315
00316 if ( img_width != 0 && img_height != 0 && win_width !=0 && win_height != 0 )
00317 {
00318 float img_aspect = img_width / img_height;
00319 float win_aspect = win_width / win_height;
00320
00321 if ( img_aspect > win_aspect )
00322 {
00323 screen_rect_->setCorners(-1.0f, 1.0f * win_aspect/img_aspect, 1.0f, -1.0f * win_aspect/img_aspect, false);
00324 }
00325 else
00326 {
00327 screen_rect_->setCorners(-1.0f * img_aspect/win_aspect, 1.0f, 1.0f * img_aspect/win_aspect, -1.0f, false);
00328 }
00329 }
00330
00331 render_panel_->updateRenderWindow();
00332 }
00333 catch (UnsupportedImageEncoding& e)
00334 {
00335 setStatus(status_levels::Error, "Image", e.what());
00336 }
00337 }
00338
00339 void ImageDisplay::onTransportEnumOptions(V_string& choices)
00340 {
00341 texture_.getAvailableTransportTypes(choices);
00342 }
00343
00344 void ImageDisplay::createProperties()
00345 {
00346 topic_property_ = property_manager_->createProperty<ROSTopicStringProperty>( "Image Topic", property_prefix_, boost::bind( &ImageDisplay::getTopic, this ),
00347 boost::bind( &ImageDisplay::setTopic, this, _1 ), parent_category_, this );
00348 setPropertyHelpText(topic_property_, "sensor_msgs::Image topic to subscribe to.");
00349 ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00350 topic_prop->setMessageType(ros::message_traits::datatype<sensor_msgs::Image>());
00351
00352 transport_property_ = property_manager_->createProperty<EditEnumProperty>("Transport Hint", property_prefix_, boost::bind(&ImageDisplay::getTransport, this),
00353 boost::bind(&ImageDisplay::setTransport, this, _1), parent_category_, this);
00354 EditEnumPropertyPtr ee_prop = transport_property_.lock();
00355 ee_prop->setOptionCallback(boost::bind(&ImageDisplay::onTransportEnumOptions, this, _1));
00356 }
00357
00358 void ImageDisplay::fixedFrameChanged()
00359 {
00360 }
00361
00362 void ImageDisplay::targetFrameChanged()
00363 {
00364
00365 }
00366
00367 void ImageDisplay::reset()
00368 {
00369 Display::reset();
00370
00371 clear();
00372 }
00373
00374 }
