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 <OgreSceneManager.h>
00038 #include <OgreSceneNode.h>
00039 #include <OgreTextureManager.h>
00040 #include <OgreViewport.h>
00041 #include <OgreTechnique.h>
00042 #include <OgreCamera.h>
00043
00044 #include <tf/transform_listener.h>
00045
00046 #include "rviz/bit_allocator.h"
00047 #include "rviz/frame_manager.h"
00048 #include "rviz/ogre_helpers/axes.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/properties/display_group_visibility_property.h"
00058 #include "rviz/load_resource.h"
00059
00060 #include <image_transport/camera_common.h>
00061
00062 #include "camera_display.h"
00063
00064 namespace rviz
00065 {
00066
00067 const QString CameraDisplay::BACKGROUND( "background" );
00068 const QString CameraDisplay::OVERLAY( "overlay" );
00069 const QString CameraDisplay::BOTH( "background and overlay" );
00070
00071 bool validateFloats(const sensor_msgs::CameraInfo& msg)
00072 {
00073 bool valid = true;
00074 valid = valid && validateFloats( msg.D );
00075 valid = valid && validateFloats( msg.K );
00076 valid = valid && validateFloats( msg.R );
00077 valid = valid && validateFloats( msg.P );
00078 return valid;
00079 }
00080
00081 CameraDisplay::CameraDisplay()
00082 : ImageDisplayBase()
00083 , texture_()
00084 , render_panel_( 0 )
00085 , caminfo_tf_filter_( 0 )
00086 , new_caminfo_( false )
00087 , force_render_( false )
00088 , caminfo_ok_(false)
00089 {
00090 image_position_property_ = new EnumProperty( "Image Rendering", BOTH,
00091 "Render the image behind all other geometry or overlay it on top, or both.",
00092 this, SLOT( forceRender() ));
00093 image_position_property_->addOption( BACKGROUND );
00094 image_position_property_->addOption( OVERLAY );
00095 image_position_property_->addOption( BOTH );
00096
00097 alpha_property_ = new FloatProperty( "Overlay Alpha", 0.5,
00098 "The amount of transparency to apply to the camera image when rendered as overlay.",
00099 this, SLOT( updateAlpha() ));
00100 alpha_property_->setMin( 0 );
00101 alpha_property_->setMax( 1 );
00102
00103 zoom_property_ = new FloatProperty( "Zoom Factor", 1.0,
00104 "Set a zoom factor below 1 to see a larger part of the world, above 1 to magnify the image.",
00105 this, SLOT( forceRender() ));
00106 zoom_property_->setMin( 0.00001 );
00107 zoom_property_->setMax( 100000 );
00108 }
00109
00110 CameraDisplay::~CameraDisplay()
00111 {
00112 if ( initialized() )
00113 {
00114 render_panel_->getRenderWindow()->removeListener( this );
00115
00116 unsubscribe();
00117 caminfo_tf_filter_->clear();
00118
00119
00120
00121 render_panel_->hide();
00122
00123
00124 delete bg_screen_rect_;
00125 delete fg_screen_rect_;
00126
00127 bg_scene_node_->getParentSceneNode()->removeAndDestroyChild( bg_scene_node_->getName() );
00128 fg_scene_node_->getParentSceneNode()->removeAndDestroyChild( fg_scene_node_->getName() );
00129
00130 delete caminfo_tf_filter_;
00131
00132 context_->visibilityBits()->freeBits(vis_bit_);
00133 }
00134 }
00135
00136 void CameraDisplay::onInitialize()
00137 {
00138 ImageDisplayBase::onInitialize();
00139
00140 caminfo_tf_filter_ = new tf::MessageFilter<sensor_msgs::CameraInfo>( *context_->getTFClient(), fixed_frame_.toStdString(),
00141 queue_size_property_->getInt(), update_nh_ );
00142
00143 bg_scene_node_ = scene_node_->createChildSceneNode();
00144 fg_scene_node_ = scene_node_->createChildSceneNode();
00145
00146 {
00147 static int count = 0;
00148 UniformStringStream ss;
00149 ss << "CameraDisplayObject" << count++;
00150
00151
00152 bg_screen_rect_ = new Ogre::Rectangle2D(true);
00153 bg_screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00154
00155 ss << "Material";
00156 bg_material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
00157 bg_material_->setDepthWriteEnabled(false);
00158
00159 bg_material_->setReceiveShadows(false);
00160 bg_material_->setDepthCheckEnabled(false);
00161
00162 bg_material_->getTechnique(0)->setLightingEnabled(false);
00163 Ogre::TextureUnitState* tu = bg_material_->getTechnique(0)->getPass(0)->createTextureUnitState();
00164 tu->setTextureName(texture_.getTexture()->getName());
00165 tu->setTextureFiltering( Ogre::TFO_NONE );
00166 tu->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, 0.0 );
00167
00168 bg_material_->setCullingMode(Ogre::CULL_NONE);
00169 bg_material_->setSceneBlending( Ogre::SBT_REPLACE );
00170
00171 Ogre::AxisAlignedBox aabInf;
00172 aabInf.setInfinite();
00173
00174 bg_screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_BACKGROUND);
00175 bg_screen_rect_->setBoundingBox(aabInf);
00176 bg_screen_rect_->setMaterial(bg_material_->getName());
00177
00178 bg_scene_node_->attachObject(bg_screen_rect_);
00179 bg_scene_node_->setVisible(false);
00180
00181
00182 fg_screen_rect_ = new Ogre::Rectangle2D(true);
00183 fg_screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00184
00185 fg_material_ = bg_material_->clone( ss.str()+"fg" );
00186 fg_screen_rect_->setBoundingBox(aabInf);
00187 fg_screen_rect_->setMaterial(fg_material_->getName());
00188
00189 fg_material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00190 fg_screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1);
00191
00192 fg_scene_node_->attachObject(fg_screen_rect_);
00193 fg_scene_node_->setVisible(false);
00194 }
00195
00196 updateAlpha();
00197
00198 render_panel_ = new RenderPanel();
00199 render_panel_->getRenderWindow()->addListener( this );
00200 render_panel_->getRenderWindow()->setAutoUpdated(false);
00201 render_panel_->getRenderWindow()->setActive( false );
00202 render_panel_->resize( 640, 480 );
00203 render_panel_->initialize( context_->getSceneManager(), context_ );
00204
00205 setAssociatedWidget( render_panel_ );
00206
00207 render_panel_->setAutoRender(false);
00208 render_panel_->setOverlaysEnabled(false);
00209 render_panel_->getCamera()->setNearClipDistance( 0.01f );
00210
00211 caminfo_tf_filter_->connectInput(caminfo_sub_);
00212 caminfo_tf_filter_->registerCallback(boost::bind(&CameraDisplay::caminfoCallback, this, _1));
00213
00214
00215 vis_bit_ = context_->visibilityBits()->allocBit();
00216 render_panel_->getViewport()->setVisibilityMask( vis_bit_ );
00217
00218 visibility_property_ = new DisplayGroupVisibilityProperty(
00219 vis_bit_, context_->getRootDisplayGroup(), this, "Visibility", true,
00220 "Changes the visibility of other Displays in the camera view.");
00221
00222 visibility_property_->setIcon( loadPixmap("package://rviz/icons/visibility.svg",true) );
00223
00224 this->addChild( visibility_property_, 0 );
00225 }
00226
00227 void CameraDisplay::preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
00228 {
00229 QString image_position = image_position_property_->getString();
00230 bg_scene_node_->setVisible( caminfo_ok_ && (image_position == BACKGROUND || image_position == BOTH) );
00231 fg_scene_node_->setVisible( caminfo_ok_ && (image_position == OVERLAY || image_position == BOTH) );
00232
00233
00234 visibility_property_->update();
00235 }
00236
00237 void CameraDisplay::postRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
00238 {
00239 bg_scene_node_->setVisible( false );
00240 fg_scene_node_->setVisible( false );
00241 }
00242
00243 void CameraDisplay::onEnable()
00244 {
00245 subscribe();
00246 render_panel_->getRenderWindow()->setActive(true);
00247 }
00248
00249 void CameraDisplay::onDisable()
00250 {
00251 render_panel_->getRenderWindow()->setActive(false);
00252 unsubscribe();
00253 clear();
00254 }
00255
00256 void CameraDisplay::subscribe()
00257 {
00258 if ( (!isEnabled()) || (topic_property_->getTopicStd().empty()) )
00259 {
00260 return;
00261 }
00262
00263 std::string target_frame = fixed_frame_.toStdString();
00264 ImageDisplayBase::enableTFFilter(target_frame);
00265
00266 ImageDisplayBase::subscribe();
00267
00268 std::string topic = topic_property_->getTopicStd();
00269 std::string caminfo_topic = image_transport::getCameraInfoTopic(topic_property_->getTopicStd());
00270
00271 try
00272 {
00273 caminfo_sub_.subscribe( update_nh_, caminfo_topic, 1 );
00274 setStatus( StatusProperty::Ok, "Camera Info", "OK" );
00275 }
00276 catch( ros::Exception& e )
00277 {
00278 setStatus( StatusProperty::Error, "Camera Info", QString( "Error subscribing: ") + e.what() );
00279 }
00280 }
00281
00282 void CameraDisplay::unsubscribe()
00283 {
00284 ImageDisplayBase::unsubscribe();
00285 caminfo_sub_.unsubscribe();
00286 }
00287
00288 void CameraDisplay::updateAlpha()
00289 {
00290 float alpha = alpha_property_->getFloat();
00291
00292 Ogre::Pass* pass = fg_material_->getTechnique( 0 )->getPass( 0 );
00293 if( pass->getNumTextureUnitStates() > 0 )
00294 {
00295 Ogre::TextureUnitState* tex_unit = pass->getTextureUnitState( 0 );
00296 tex_unit->setAlphaOperation( Ogre::LBX_MODULATE, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha );
00297 }
00298 else
00299 {
00300 fg_material_->setAmbient( Ogre::ColourValue( 0.0f, 1.0f, 1.0f, alpha ));
00301 fg_material_->setDiffuse( Ogre::ColourValue( 0.0f, 1.0f, 1.0f, alpha ));
00302 }
00303
00304 force_render_ = true;
00305 context_->queueRender();
00306 }
00307
00308 void CameraDisplay::forceRender()
00309 {
00310 force_render_ = true;
00311 context_->queueRender();
00312 }
00313
00314 void CameraDisplay::updateQueueSize()
00315 {
00316 caminfo_tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() );
00317 ImageDisplayBase::updateQueueSize();
00318 }
00319
00320 void CameraDisplay::clear()
00321 {
00322 texture_.clear();
00323 force_render_ = true;
00324 context_->queueRender();
00325
00326 new_caminfo_ = false;
00327 current_caminfo_.reset();
00328
00329 setStatus( StatusProperty::Warn, "Camera Info",
00330 "No CameraInfo received on [" + QString::fromStdString( caminfo_sub_.getTopic() ) + "]. Topic may not exist.");
00331 setStatus( StatusProperty::Warn, "Image", "No Image received");
00332
00333 render_panel_->getCamera()->setPosition( Ogre::Vector3( 999999, 999999, 999999 ));
00334 }
00335
00336 void CameraDisplay::update( float wall_dt, float ros_dt )
00337 {
00338 try
00339 {
00340 if( texture_.update() || force_render_ )
00341 {
00342 caminfo_ok_ = updateCamera();
00343 force_render_ = false;
00344 }
00345 }
00346 catch( UnsupportedImageEncoding& e )
00347 {
00348 setStatus( StatusProperty::Error, "Image", e.what() );
00349 }
00350
00351 render_panel_->getRenderWindow()->update();
00352 }
00353
00354 bool CameraDisplay::updateCamera()
00355 {
00356 sensor_msgs::CameraInfo::ConstPtr info;
00357 sensor_msgs::Image::ConstPtr image;
00358 {
00359 boost::mutex::scoped_lock lock( caminfo_mutex_ );
00360
00361 info = current_caminfo_;
00362 image = texture_.getImage();
00363 }
00364
00365 if( !info || !image )
00366 {
00367 return false;
00368 }
00369
00370 if( !validateFloats( *info ))
00371 {
00372 setStatus( StatusProperty::Error, "Camera Info", "Contains invalid floating point values (nans or infs)" );
00373 return false;
00374 }
00375
00376
00377 ros::Time rviz_time = context_->getFrameManager()->getTime();
00378 if ( context_->getFrameManager()->getSyncMode() == FrameManager::SyncExact &&
00379 rviz_time != image->header.stamp )
00380 {
00381 std::ostringstream s;
00382 s << "Time-syncing active and no image at timestamp " << rviz_time.toSec() << ".";
00383 setStatus( StatusProperty::Warn, "Time", s.str().c_str() );
00384 return false;
00385 }
00386
00387 Ogre::Vector3 position;
00388 Ogre::Quaternion orientation;
00389 context_->getFrameManager()->getTransform( image->header.frame_id, image->header.stamp, position, orientation );
00390
00391
00392
00393
00394 orientation = orientation * Ogre::Quaternion( Ogre::Degree( 180 ), Ogre::Vector3::UNIT_X );
00395
00396 float img_width = info->width;
00397 float img_height = info->height;
00398
00399
00400 if( img_width == 0 )
00401 {
00402 ROS_DEBUG( "Malformed CameraInfo on camera [%s], width = 0", qPrintable( getName() ));
00403 img_width = texture_.getWidth();
00404 }
00405
00406 if (img_height == 0)
00407 {
00408 ROS_DEBUG( "Malformed CameraInfo on camera [%s], height = 0", qPrintable( getName() ));
00409 img_height = texture_.getHeight();
00410 }
00411
00412 if( img_height == 0.0 || img_width == 0.0 )
00413 {
00414 setStatus( StatusProperty::Error, "Camera Info",
00415 "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)" );
00416 return false;
00417 }
00418
00419 double fx = info->P[0];
00420 double fy = info->P[5];
00421
00422 float win_width = render_panel_->width();
00423 float win_height = render_panel_->height();
00424 float zoom_x = zoom_property_->getFloat();
00425 float zoom_y = zoom_x;
00426
00427
00428 if( win_width != 0 && win_height != 0 )
00429 {
00430 float img_aspect = (img_width/fx) / (img_height/fy);
00431 float win_aspect = win_width / win_height;
00432
00433 if ( img_aspect > win_aspect )
00434 {
00435 zoom_y = zoom_y / img_aspect * win_aspect;
00436 }
00437 else
00438 {
00439 zoom_x = zoom_x / win_aspect * img_aspect;
00440 }
00441 }
00442
00443
00444 double tx = -1 * (info->P[3] / fx);
00445 Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
00446 position = position + (right * tx);
00447
00448 double ty = -1 * (info->P[7] / fy);
00449 Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y;
00450 position = position + (down * ty);
00451
00452 if( !validateFloats( position ))
00453 {
00454 setStatus( StatusProperty::Error, "Camera Info", "CameraInfo/P resulted in an invalid position calculation (nans or infs)" );
00455 return false;
00456 }
00457
00458 render_panel_->getCamera()->setPosition( position );
00459 render_panel_->getCamera()->setOrientation( orientation );
00460
00461
00462 double cx = info->P[2];
00463 double cy = info->P[6];
00464
00465 double far_plane = 100;
00466 double near_plane = 0.01;
00467
00468 Ogre::Matrix4 proj_matrix;
00469 proj_matrix = Ogre::Matrix4::ZERO;
00470
00471 proj_matrix[0][0]= 2.0 * fx/img_width * zoom_x;
00472 proj_matrix[1][1]= 2.0 * fy/img_height * zoom_y;
00473
00474 proj_matrix[0][2]= 2.0 * (0.5 - cx/img_width) * zoom_x;
00475 proj_matrix[1][2]= 2.0 * (cy/img_height - 0.5) * zoom_y;
00476
00477 proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane);
00478 proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane);
00479
00480 proj_matrix[3][2]= -1;
00481
00482 render_panel_->getCamera()->setCustomProjectionMatrix( true, proj_matrix );
00483
00484 setStatus( StatusProperty::Ok, "Camera Info", "OK" );
00485
00486 #if 0
00487 static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01);
00488 debug_axes->setPosition(position);
00489 debug_axes->setOrientation(orientation);
00490 #endif
00491
00492
00493 bg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y );
00494 fg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y );
00495
00496 Ogre::AxisAlignedBox aabInf;
00497 aabInf.setInfinite();
00498 bg_screen_rect_->setBoundingBox( aabInf );
00499 fg_screen_rect_->setBoundingBox( aabInf );
00500
00501 setStatus( StatusProperty::Ok, "Time", "ok" );
00502 setStatus( StatusProperty::Ok, "Camera Info", "ok" );
00503
00504 return true;
00505 }
00506
00507 void CameraDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg)
00508 {
00509 texture_.addMessage(msg);
00510 }
00511
00512 void CameraDisplay::caminfoCallback( const sensor_msgs::CameraInfo::ConstPtr& msg )
00513 {
00514 boost::mutex::scoped_lock lock( caminfo_mutex_ );
00515 current_caminfo_ = msg;
00516 new_caminfo_ = true;
00517 }
00518
00519 void CameraDisplay::fixedFrameChanged()
00520 {
00521 std::string targetFrame = fixed_frame_.toStdString();
00522 caminfo_tf_filter_->setTargetFrame(targetFrame);
00523 ImageDisplayBase::fixedFrameChanged();
00524 }
00525
00526 void CameraDisplay::reset()
00527 {
00528 ImageDisplayBase::reset();
00529 clear();
00530 }
00531
00532 }
00533
00534 #include <pluginlib/class_list_macros.h>
00535 PLUGINLIB_EXPORT_CLASS( rviz::CameraDisplay, rviz::Display )