camera_display.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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     //workaround. delete results in a later crash
00121     render_panel_->hide();
00122     //delete render_panel_;
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     //background rectangle
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     //overlay rectangle
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   //context_->getFrameManager()->registerFilterForTransformStatusCheck(caminfo_tf_filter_, this);
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   // set view flags on all displays
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   // if we're in 'exact' time mode, only show image if the time is exactly right
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   //printf( "CameraDisplay:updateCamera(): pos = %.2f, %.2f, %.2f.\n", position.x, position.y, position.z );
00392 
00393   // convert vision (Z-forward) frame to ogre frame (Z-out)
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   // If the image width is 0 due to a malformed caminfo, try to grab the width from the image.
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   // Preserve aspect ratio
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   // Add the camera's translation relative to the left camera (from P[3]);
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   // calculate the projection matrix
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   //adjust the image rectangles to fit the zoom & aspect ratio
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 } // namespace rviz
00533 
00534 #include <pluginlib/class_list_macros.h>
00535 PLUGINLIB_EXPORT_CLASS( rviz::CameraDisplay, rviz::Display )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27