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


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32