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/frame_manager.h"
00046 #include "rviz/ogre_helpers/axes.h"
00047 #include "rviz/panel_dock_widget.h"
00048 #include "rviz/properties/editable_enum_property.h"
00049 #include "rviz/properties/enum_property.h"
00050 #include "rviz/properties/float_property.h"
00051 #include "rviz/properties/int_property.h"
00052 #include "rviz/properties/ros_topic_property.h"
00053 #include "rviz/render_panel.h"
00054 #include "rviz/uniform_string_stream.h"
00055 #include "rviz/validate_floats.h"
00056 #include "rviz/display_context.h"
00057 #include "rviz/window_manager_interface.h"
00058
00059 #include "camera_display.h"
00060
00061 namespace rviz
00062 {
00063
00064 const QString CameraDisplay::BACKGROUND( "background" );
00065 const QString CameraDisplay::OVERLAY( "overlay" );
00066 const QString CameraDisplay::BOTH( "background and overlay" );
00067
00068 bool validateFloats(const sensor_msgs::CameraInfo& msg)
00069 {
00070 bool valid = true;
00071 valid = valid && validateFloats( msg.D );
00072 valid = valid && validateFloats( msg.K );
00073 valid = valid && validateFloats( msg.R );
00074 valid = valid && validateFloats( msg.P );
00075 return valid;
00076 }
00077
00078 CameraDisplay::CameraDisplay()
00079 : Display()
00080 , caminfo_tf_filter_( 0 )
00081 , new_caminfo_( false )
00082 , texture_( update_nh_ )
00083 , render_panel_( 0 )
00084 , force_render_( false )
00085 , panel_container_( 0 )
00086 {
00087 topic_property_ = new RosTopicProperty( "Image Topic", "",
00088 QString::fromStdString( ros::message_traits::datatype<sensor_msgs::Image>() ),
00089 "sensor_msgs::Image topic to subscribe to. "
00090 "The topic must be a well-formed <strong>camera</strong> topic, "
00091 "and in order to work properly must have a matching <strong>camera_info<strong> topic.",
00092 this, SLOT( updateTopic() ));
00093
00094 transport_property_ = new EditableEnumProperty( "Transport Hint", "raw",
00095 "Preferred method of sending images.",
00096 this, SLOT( updateTransport() ));
00097 connect( transport_property_, SIGNAL( requestOptions( QStringList* )),
00098 this, SLOT( fillTransportOptionList( QStringList* )));
00099
00100 image_position_property_ = new EnumProperty( "Image Rendering", BOTH,
00101 "Render the image behind all other geometry or overlay it on top, or both.",
00102 this, SLOT( forceRender() ));
00103 image_position_property_->addOption( BACKGROUND );
00104 image_position_property_->addOption( OVERLAY );
00105 image_position_property_->addOption( BOTH );
00106
00107 alpha_property_ = new FloatProperty( "Overlay Alpha", 0.5,
00108 "The amount of transparency to apply to the camera image when rendered as overlay.",
00109 this, SLOT( updateAlpha() ));
00110 alpha_property_->setMin( 0 );
00111 alpha_property_->setMax( 1 );
00112
00113 zoom_property_ = new FloatProperty( "Zoom Factor", 1.0,
00114 "Set a zoom factor below 1 to see a larger part of the world, above 1 to magnify the image.",
00115 this, SLOT( forceRender() ));
00116 zoom_property_->setMin( 0.00001 );
00117 zoom_property_->setMax( 100000 );
00118
00119 queue_size_property_ = new IntProperty( "Queue Size", 2,
00120 "Advanced: set the size of the incoming message queue. Increasing this "
00121 "is useful if your incoming TF data is delayed significantly from your"
00122 " camera data, but it can greatly increase memory usage if the messages are big.",
00123 this, SLOT( updateQueueSize() ));
00124 queue_size_property_->setMin( 1 );
00125 }
00126
00127 CameraDisplay::~CameraDisplay()
00128 {
00129 unsubscribe();
00130 caminfo_tf_filter_->clear();
00131
00132 if( render_panel_ )
00133 {
00134 if( panel_container_ )
00135 {
00136 delete panel_container_;
00137 }
00138 else
00139 {
00140 delete render_panel_;
00141 }
00142 }
00143
00144 delete bg_screen_rect_;
00145 delete fg_screen_rect_;
00146
00147 bg_scene_node_->getParentSceneNode()->removeAndDestroyChild( bg_scene_node_->getName() );
00148 fg_scene_node_->getParentSceneNode()->removeAndDestroyChild( fg_scene_node_->getName() );
00149
00150 delete caminfo_tf_filter_;
00151 }
00152
00153 void CameraDisplay::onInitialize()
00154 {
00155 caminfo_tf_filter_ = new tf::MessageFilter<sensor_msgs::CameraInfo>( *context_->getTFClient(), fixed_frame_.toStdString(),
00156 queue_size_property_->getInt(), update_nh_ );
00157
00158 bg_scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00159 fg_scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00160
00161 {
00162 static int count = 0;
00163 UniformStringStream ss;
00164 ss << "CameraDisplayObject" << count++;
00165
00166
00167 bg_screen_rect_ = new Ogre::Rectangle2D(true);
00168 bg_screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00169
00170 ss << "Material";
00171 bg_material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
00172 bg_material_->setDepthWriteEnabled(false);
00173
00174 bg_material_->setReceiveShadows(false);
00175 bg_material_->setDepthCheckEnabled(false);
00176
00177 bg_material_->getTechnique(0)->setLightingEnabled(false);
00178 Ogre::TextureUnitState* tu = bg_material_->getTechnique(0)->getPass(0)->createTextureUnitState();
00179 tu->setTextureName(texture_.getTexture()->getName());
00180 tu->setTextureFiltering( Ogre::TFO_NONE );
00181 tu->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, 0.0 );
00182
00183 bg_material_->setCullingMode(Ogre::CULL_NONE);
00184 bg_material_->setSceneBlending( Ogre::SBT_REPLACE );
00185
00186 Ogre::AxisAlignedBox aabInf;
00187 aabInf.setInfinite();
00188
00189 bg_screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_BACKGROUND);
00190 bg_screen_rect_->setBoundingBox(aabInf);
00191 bg_screen_rect_->setMaterial(bg_material_->getName());
00192
00193 bg_scene_node_->attachObject(bg_screen_rect_);
00194 bg_scene_node_->setVisible(false);
00195
00196
00197 fg_screen_rect_ = new Ogre::Rectangle2D(true);
00198 fg_screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00199
00200 fg_material_ = bg_material_->clone( ss.str()+"fg" );
00201 fg_screen_rect_->setBoundingBox(aabInf);
00202 fg_screen_rect_->setMaterial(fg_material_->getName());
00203
00204 fg_material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00205 fg_screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1);
00206
00207 fg_scene_node_->attachObject(fg_screen_rect_);
00208 fg_scene_node_->setVisible(false);
00209 }
00210
00211 updateAlpha();
00212
00213 render_panel_ = new RenderPanel();
00214 render_panel_->getRenderWindow()->addListener( this );
00215 render_panel_->getRenderWindow()->setAutoUpdated(false);
00216 render_panel_->getRenderWindow()->setActive( false );
00217 render_panel_->resize( 640, 480 );
00218 render_panel_->initialize( context_->getSceneManager(), context_ );
00219
00220 WindowManagerInterface* wm = context_->getWindowManager();
00221 if( wm )
00222 {
00223 panel_container_ = wm->addPane( getName().toStdString(), render_panel_);
00224 }
00225 render_panel_->setAutoRender(false);
00226 render_panel_->setOverlaysEnabled(false);
00227 render_panel_->getCamera()->setNearClipDistance( 0.01f );
00228
00229 caminfo_tf_filter_->connectInput(caminfo_sub_);
00230 caminfo_tf_filter_->registerCallback(boost::bind(&CameraDisplay::caminfoCallback, this, _1));
00231 context_->getFrameManager()->registerFilterForTransformStatusCheck(caminfo_tf_filter_, this);
00232
00233 if( panel_container_ )
00234 {
00235 connect( panel_container_, SIGNAL( visibilityChanged( bool ) ), this, SLOT( setEnabled( bool )));
00236 }
00237 }
00238
00239 void CameraDisplay::preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
00240 {
00241 QString image_position = image_position_property_->getString();
00242 bg_scene_node_->setVisible( image_position == BACKGROUND || image_position == BOTH );
00243 fg_scene_node_->setVisible( image_position == OVERLAY || image_position == BOTH );
00244 }
00245
00246 void CameraDisplay::postRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
00247 {
00248 bg_scene_node_->setVisible( false );
00249 fg_scene_node_->setVisible( false );
00250 }
00251
00252 void CameraDisplay::onEnable()
00253 {
00254 subscribe();
00255 if( render_panel_->parentWidget() == 0 )
00256 {
00257 render_panel_->show();
00258 }
00259 else
00260 {
00261 panel_container_->show();
00262 }
00263
00264 render_panel_->getRenderWindow()->setActive(true);
00265 }
00266
00267 void CameraDisplay::onDisable()
00268 {
00269 render_panel_->getRenderWindow()->setActive(false);
00270
00271 if( render_panel_->parentWidget() == 0 )
00272 {
00273 if( render_panel_->isVisible() )
00274 {
00275 render_panel_->hide();
00276 }
00277 }
00278 else
00279 {
00280 if( panel_container_->isVisible() )
00281 {
00282 panel_container_->hide();
00283 }
00284 }
00285
00286 unsubscribe();
00287
00288 clear();
00289 }
00290
00291 void CameraDisplay::subscribe()
00292 {
00293 if ( !isEnabled() )
00294 {
00295 return;
00296 }
00297
00298 std::string topic = topic_property_->getTopicStd();
00299
00300 try
00301 {
00302 texture_.setTopic( topic );
00303 setStatus( StatusProperty::Ok, "Topic", "OK" );
00304 }
00305 catch( ros::Exception& e )
00306 {
00307 setStatus( StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what() );
00308 }
00309
00310
00311 std::string caminfo_topic = "camera_info";
00312 size_t pos = topic.rfind( '/' );
00313 if( pos != std::string::npos )
00314 {
00315 std::string ns = topic;
00316 ns.erase( pos );
00317
00318 caminfo_topic = ns + "/" + caminfo_topic;
00319 }
00320
00321 try
00322 {
00323 caminfo_sub_.subscribe( update_nh_, caminfo_topic, 1 );
00324 setStatus( StatusProperty::Ok, "Camera Info Topic", "OK" );
00325 }
00326 catch( ros::Exception& e )
00327 {
00328 setStatus( StatusProperty::Error, "Camera Info Topic", QString( "Error subscribing: ") + e.what() );
00329 }
00330 }
00331
00332 void CameraDisplay::unsubscribe()
00333 {
00334 texture_.setTopic( "" );
00335 caminfo_sub_.unsubscribe();
00336 }
00337
00338 void CameraDisplay::updateAlpha()
00339 {
00340 float alpha = alpha_property_->getFloat();
00341
00342 Ogre::Pass* pass = fg_material_->getTechnique( 0 )->getPass( 0 );
00343 if( pass->getNumTextureUnitStates() > 0 )
00344 {
00345 Ogre::TextureUnitState* tex_unit = pass->getTextureUnitState( 0 );
00346 tex_unit->setAlphaOperation( Ogre::LBX_MODULATE, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha );
00347 }
00348 else
00349 {
00350 fg_material_->setAmbient( Ogre::ColourValue( 0.0f, 1.0f, 1.0f, alpha ));
00351 fg_material_->setDiffuse( Ogre::ColourValue( 0.0f, 1.0f, 1.0f, alpha ));
00352 }
00353
00354 force_render_ = true;
00355 context_->queueRender();
00356 }
00357
00358 void CameraDisplay::forceRender()
00359 {
00360 force_render_ = true;
00361 context_->queueRender();
00362 }
00363
00364 void CameraDisplay::updateQueueSize()
00365 {
00366 uint32_t size = queue_size_property_->getInt();
00367 texture_.setQueueSize( size );
00368 caminfo_tf_filter_->setQueueSize( size );
00369 }
00370
00371 void CameraDisplay::updateTopic()
00372 {
00373 unsubscribe();
00374 clear();
00375 subscribe();
00376 }
00377
00378 void CameraDisplay::updateTransport()
00379 {
00380 texture_.setTransportType( transport_property_->getStdString() );
00381 }
00382
00383 void CameraDisplay::clear()
00384 {
00385 texture_.clear();
00386 force_render_ = true;
00387 context_->queueRender();
00388
00389 new_caminfo_ = false;
00390 current_caminfo_.reset();
00391
00392 setStatus( StatusProperty::Warn, "CameraInfo",
00393 "No CameraInfo received on [" + QString::fromStdString( caminfo_sub_.getTopic() ) + "]. Topic may not exist.");
00394 setStatus( StatusProperty::Warn, "Image", "No Image received");
00395
00396 render_panel_->getCamera()->setPosition( Ogre::Vector3( 999999, 999999, 999999 ));
00397 }
00398
00399 void CameraDisplay::updateStatus()
00400 {
00401 if( texture_.getImageCount() == 0 )
00402 {
00403 setStatus( StatusProperty::Warn, "Image", "No image received" );
00404 }
00405 else
00406 {
00407 setStatus( StatusProperty::Ok, "Image", QString::number( texture_.getImageCount() ) + " images received" );
00408 }
00409 }
00410
00411 void CameraDisplay::update( float wall_dt, float ros_dt )
00412 {
00413 updateStatus();
00414
00415 try
00416 {
00417 if( texture_.update() || force_render_ )
00418 {
00419
00420
00421
00422
00423
00424
00425 updateCamera();
00426 render_panel_->getRenderWindow()->update();
00427
00428
00429 force_render_ = false;
00430 }
00431 }
00432 catch( UnsupportedImageEncoding& e )
00433 {
00434 setStatus( StatusProperty::Error, "Image", e.what() );
00435 }
00436 }
00437
00438 void CameraDisplay::updateCamera()
00439 {
00440 sensor_msgs::CameraInfo::ConstPtr info;
00441 sensor_msgs::Image::ConstPtr image;
00442 {
00443 boost::mutex::scoped_lock lock( caminfo_mutex_ );
00444
00445 info = current_caminfo_;
00446 image = texture_.getImage();
00447 }
00448
00449 if( !info || !image )
00450 {
00451 return;
00452 }
00453
00454 if( !validateFloats( *info ))
00455 {
00456 setStatus( StatusProperty::Error, "CameraInfo", "Contains invalid floating point values (nans or infs)" );
00457 return;
00458 }
00459
00460 Ogre::Vector3 position;
00461 Ogre::Quaternion orientation;
00462 context_->getFrameManager()->getTransform( image->header, position, orientation );
00463
00464
00465 orientation = orientation * Ogre::Quaternion( Ogre::Degree( 180 ), Ogre::Vector3::UNIT_X );
00466
00467 float img_width = info->width;
00468 float img_height = info->height;
00469
00470
00471 if( img_width == 0 )
00472 {
00473 ROS_DEBUG( "Malformed CameraInfo on camera [%s], width = 0", qPrintable( getName() ));
00474
00475 img_width = texture_.getWidth();
00476 }
00477
00478 if (img_height == 0)
00479 {
00480 ROS_DEBUG( "Malformed CameraInfo on camera [%s], height = 0", qPrintable( getName() ));
00481
00482 img_height = texture_.getHeight();
00483 }
00484
00485 if( img_height == 0.0 || img_width == 0.0 )
00486 {
00487 setStatus( StatusProperty::Error, "CameraInfo",
00488 "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)" );
00489 return;
00490 }
00491
00492 double fx = info->P[0];
00493 double fy = info->P[5];
00494
00495 float win_width = render_panel_->width();
00496 float win_height = render_panel_->height();
00497 float zoom_x = zoom_property_->getFloat();
00498 float zoom_y = zoom_x;
00499
00500
00501 if( win_width != 0 && win_height != 0 )
00502 {
00503 float img_aspect = (img_width/fx) / (img_height/fy);
00504 float win_aspect = win_width / win_height;
00505
00506 if ( img_aspect > win_aspect )
00507 {
00508 zoom_y = zoom_y / img_aspect * win_aspect;
00509 }
00510 else
00511 {
00512 zoom_x = zoom_x / win_aspect * img_aspect;
00513 }
00514 }
00515
00516
00517 double tx = -1 * (info->P[3] / fx);
00518 Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
00519 position = position + (right * tx);
00520
00521 double ty = -1 * (info->P[7] / fy);
00522 Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y;
00523 position = position + (down * ty);
00524
00525 if( !validateFloats( position ))
00526 {
00527 setStatus( StatusProperty::Error, "CameraInfo", "CameraInfo/P resulted in an invalid position calculation (nans or infs)" );
00528 return;
00529 }
00530
00531 render_panel_->getCamera()->setPosition( position );
00532 render_panel_->getCamera()->setOrientation( orientation );
00533
00534
00535 double cx = info->P[2];
00536 double cy = info->P[6];
00537
00538 double far_plane = 100;
00539 double near_plane = 0.01;
00540
00541 Ogre::Matrix4 proj_matrix;
00542 proj_matrix = Ogre::Matrix4::ZERO;
00543
00544 proj_matrix[0][0]= 2.0 * fx/img_width * zoom_x;
00545 proj_matrix[1][1]= 2.0 * fy/img_height * zoom_y;
00546
00547 proj_matrix[0][2]= 2.0 * (0.5 - cx/img_width) * zoom_x;
00548 proj_matrix[1][2]= 2.0 * (cy/img_height - 0.5) * zoom_y;
00549
00550 proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane);
00551 proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane);
00552
00553 proj_matrix[3][2]= -1;
00554
00555 render_panel_->getCamera()->setCustomProjectionMatrix( true, proj_matrix );
00556
00557 setStatus( StatusProperty::Ok, "CameraInfo", "OK" );
00558
00559 #if 0
00560 static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01);
00561 debug_axes->setPosition(position);
00562 debug_axes->setOrientation(orientation);
00563 #endif
00564
00565
00566 bg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y );
00567 fg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y );
00568
00569 Ogre::AxisAlignedBox aabInf;
00570 aabInf.setInfinite();
00571 bg_screen_rect_->setBoundingBox( aabInf );
00572 fg_screen_rect_->setBoundingBox( aabInf );
00573 }
00574
00575 void CameraDisplay::caminfoCallback( const sensor_msgs::CameraInfo::ConstPtr& msg )
00576 {
00577 boost::mutex::scoped_lock lock( caminfo_mutex_ );
00578 current_caminfo_ = msg;
00579 new_caminfo_ = true;
00580 }
00581
00582 void CameraDisplay::fillTransportOptionList( QStringList* qchoices_out )
00583 {
00584 V_string choices;
00585 texture_.getAvailableTransportTypes( choices );
00586 for( size_t i = 0; i < choices.size(); i++ )
00587 {
00588 qchoices_out->append( QString::fromStdString( choices[ i ]));
00589 }
00590 }
00591
00592 void CameraDisplay::fixedFrameChanged()
00593 {
00594 caminfo_tf_filter_->setTargetFrame( fixed_frame_.toStdString() );
00595 texture_.setFrame( fixed_frame_.toStdString(), context_->getTFClient() );
00596 }
00597
00598 void CameraDisplay::reset()
00599 {
00600 Display::reset();
00601 clear();
00602 }
00603
00604 void CameraDisplay::setName( const QString& name )
00605 {
00606 Display::setName( name );
00607 if( panel_container_ )
00608 {
00609 panel_container_->setWindowTitle( name );
00610 panel_container_->setObjectName( name );
00611 }
00612 else
00613 {
00614 render_panel_->setWindowTitle( name );
00615 }
00616 }
00617
00618 }
00619
00620 #include <pluginlib/class_list_macros.h>
00621 PLUGINLIB_DECLARE_CLASS( rviz, Camera, rviz::CameraDisplay, rviz::Display )