overlay_camera_display.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include <boost/bind.hpp>
00037 
00038 #include <OgreManualObject.h>
00039 #include <OgreMaterialManager.h>
00040 #include <OgreRectangle2D.h>
00041 #include <OgreRenderSystem.h>
00042 #include <OgreRenderWindow.h>
00043 #include <OgreSceneManager.h>
00044 #include <OgreSceneNode.h>
00045 #include <OgreTextureManager.h>
00046 #include <OgreViewport.h>
00047 #include <OgreTechnique.h>
00048 #include <OgreCamera.h>
00049 #include <OgrePixelFormat.h>
00050 #include <OGRE/OgreHardwarePixelBuffer.h>
00051 #include <OGRE/OgreTechnique.h>
00052 #include <tf/transform_listener.h>
00053 
00054 #include "rviz/bit_allocator.h"
00055 #include "rviz/frame_manager.h"
00056 #include "rviz/ogre_helpers/axes.h"
00057 #include "rviz/properties/enum_property.h"
00058 #include "rviz/properties/float_property.h"
00059 #include "rviz/properties/int_property.h"
00060 #include "rviz/properties/ros_topic_property.h"
00061 #include "rviz/render_panel.h"
00062 #include "rviz/uniform_string_stream.h"
00063 #include "rviz/validate_floats.h"
00064 #include "rviz/display_context.h"
00065 #include "rviz/properties/display_group_visibility_property.h"
00066 #include "rviz/load_resource.h"
00067 
00068 #include <image_transport/camera_common.h>
00069 #include "overlay_camera_display.h"
00070 
00071 namespace jsk_rviz_plugins
00072 {
00073 using namespace rviz;
00074 const QString OverlayCameraDisplay::BACKGROUND( "background" );
00075 const QString OverlayCameraDisplay::OVERLAY( "overlay" );
00076 const QString OverlayCameraDisplay::BOTH( "background and overlay" );
00077 
00078 bool validateFloats(const sensor_msgs::CameraInfo& msg)
00079 {
00080   bool valid = true;
00081   valid = valid && rviz::validateFloats( msg.D );
00082   valid = valid && rviz::validateFloats( msg.K );
00083   valid = valid && rviz::validateFloats( msg.R );
00084   valid = valid && rviz::validateFloats( msg.P );
00085   return valid;
00086 }
00087 
00088 OverlayCameraDisplay::OverlayCameraDisplay()
00089   : ImageDisplayBase()
00090   , texture_()
00091   , render_panel_( 0 )
00092   , caminfo_tf_filter_( 0 )
00093   , new_caminfo_( false )
00094   , force_render_( false )
00095   , caminfo_ok_(false)
00096 {
00097   image_position_property_ = new EnumProperty( "Image Rendering", BOTH,
00098                                                "Render the image behind all other geometry or overlay it on top, or both.",
00099                                                this, SLOT( forceRender() ));
00100   image_position_property_->addOption( BACKGROUND );
00101   image_position_property_->addOption( OVERLAY );
00102   image_position_property_->addOption( BOTH );
00103 
00104   alpha_property_ = new FloatProperty( "Overlay Alpha", 0.5,
00105                                        "The amount of transparency to apply to the camera image when rendered as overlay.",
00106                                        this, SLOT( updateAlpha() ));
00107   alpha_property_->setMin( 0 );
00108   alpha_property_->setMax( 1 );
00109   
00110 
00111   zoom_property_ = new FloatProperty( "Zoom Factor", 1.0,
00112                                       "Set a zoom factor below 1 to see a larger part of the world, above 1 to magnify the image.",
00113                                       this, SLOT( forceRender() ));
00114   zoom_property_->setMin( 0.00001 );
00115   zoom_property_->setMax( 100000 );
00116 
00117   width_property_ = new IntProperty("width", 640,
00118                                     "width of overlay image",
00119                                     this, SLOT(updateWidth()));
00120   height_property_ = new IntProperty("height", 480,
00121                                     "height of overlay image",
00122                                     this, SLOT(updateHeight()));
00123   left_property_ = new IntProperty("left", 0,
00124                                    "left positoin of overlay image",
00125                                    this, SLOT(updateLeft()));
00126   top_property_ = new IntProperty("top", 0,
00127                                   "top positoin of overlay image",
00128                                    this, SLOT(updateTop()));
00129   texture_alpha_property_ = new FloatProperty("texture alpha", 0.8,
00130                                               "texture alpha",
00131                                               this, SLOT(updateTextureAlpha()));
00132   texture_alpha_property_->setMin(0.0);
00133   texture_alpha_property_->setMax(1.0);
00134 }
00135 
00136 OverlayCameraDisplay::~OverlayCameraDisplay()
00137 {
00138   if ( initializedp_ )
00139   {
00140     render_panel_->getRenderWindow()->removeListener( this );
00141 
00142     unsubscribe();
00143     caminfo_tf_filter_->clear();
00144 
00145 
00146     //workaround. delete results in a later crash
00147     render_panel_->hide();
00148     //delete render_panel_;
00149 
00150     delete bg_screen_rect_;
00151     delete fg_screen_rect_;
00152 
00153     bg_scene_node_->getParentSceneNode()->removeAndDestroyChild( bg_scene_node_->getName() );
00154     fg_scene_node_->getParentSceneNode()->removeAndDestroyChild( fg_scene_node_->getName() );
00155 
00156     delete caminfo_tf_filter_;
00157 
00158     context_->visibilityBits()->freeBits(vis_bit_);
00159   }
00160 }
00161 
00162 void OverlayCameraDisplay::onInitialize()
00163 {
00164   ImageDisplayBase::onInitialize();
00165   
00166   caminfo_tf_filter_ = new tf::MessageFilter<sensor_msgs::CameraInfo>(
00167     *context_->getTFClient(), fixed_frame_.toStdString(),
00168     queue_size_property_->getInt(), update_nh_ );
00169 
00170   bg_scene_node_ = scene_node_->createChildSceneNode();
00171   fg_scene_node_ = scene_node_->createChildSceneNode();
00172 
00173   {
00174     static int count = 0;
00175     UniformStringStream ss;
00176     ss << "OverlayCameraDisplayObject" << count++;
00177 
00178     //background rectangle
00179     bg_screen_rect_ = new Ogre::Rectangle2D(true);
00180     bg_screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00181 
00182     ss << "Material";
00183     bg_material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
00184     bg_material_->setDepthWriteEnabled(false);
00185 
00186     bg_material_->setReceiveShadows(false);
00187     bg_material_->setDepthCheckEnabled(false);
00188 
00189     bg_material_->getTechnique(0)->setLightingEnabled(false);
00190     Ogre::TextureUnitState* tu = bg_material_->getTechnique(0)->getPass(0)->createTextureUnitState();
00191     tu->setTextureName(texture_.getTexture()->getName());
00192     tu->setTextureFiltering( Ogre::TFO_NONE );
00193     tu->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, 0.0 );
00194 
00195     bg_material_->setCullingMode(Ogre::CULL_NONE);
00196     bg_material_->setSceneBlending( Ogre::SBT_REPLACE );
00197 
00198     Ogre::AxisAlignedBox aabInf;
00199     aabInf.setInfinite();
00200 
00201     bg_screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_BACKGROUND);
00202     bg_screen_rect_->setBoundingBox(aabInf);
00203     bg_screen_rect_->setMaterial(bg_material_->getName());
00204 
00205     bg_scene_node_->attachObject(bg_screen_rect_);
00206     bg_scene_node_->setVisible(false);
00207 
00208     //overlay rectangle
00209     fg_screen_rect_ = new Ogre::Rectangle2D(true);
00210     fg_screen_rect_->setCorners(-1.0f, 1.0f, 1.0f, -1.0f);
00211 
00212     fg_material_ = bg_material_->clone( ss.str()+"fg" );
00213     fg_screen_rect_->setBoundingBox(aabInf);
00214     fg_screen_rect_->setMaterial(fg_material_->getName());
00215 
00216     fg_material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00217     fg_screen_rect_->setRenderQueueGroup(Ogre::RENDER_QUEUE_OVERLAY - 1);
00218 
00219     fg_scene_node_->attachObject(fg_screen_rect_);
00220     fg_scene_node_->setVisible(false);
00221   }
00222 
00223   updateAlpha();
00224 
00225   updateWidth();
00226   updateHeight();
00227   updateLeft();
00228   updateTop();
00229   updateTextureAlpha();
00230   
00231   render_panel_ = new RenderPanel();
00232   render_panel_->getRenderWindow()->addListener( this );
00233   render_panel_->getRenderWindow()->setAutoUpdated(false);
00234   render_panel_->getRenderWindow()->setActive( false );
00235   render_panel_->resize( 640, 480 );
00236   render_panel_->initialize( context_->getSceneManager(), context_ );
00237 
00238   //setAssociatedWidget( render_panel_ );
00239 
00240   render_panel_->setAutoRender(false);
00241   render_panel_->setOverlaysEnabled(false);
00242   render_panel_->getCamera()->setNearClipDistance( 0.01f );
00243 
00244   caminfo_tf_filter_->connectInput(caminfo_sub_);
00245   caminfo_tf_filter_->registerCallback(boost::bind(&OverlayCameraDisplay::caminfoCallback, this, _1));
00246   //context_->getFrameManager()->registerFilterForTransformStatusCheck(caminfo_tf_filter_, this);
00247 
00248   vis_bit_ = context_->visibilityBits()->allocBit();
00249   render_panel_->getViewport()->setVisibilityMask( vis_bit_ );
00250 
00251   visibility_property_ = new DisplayGroupVisibilityProperty(
00252       vis_bit_, context_->getRootDisplayGroup(), this, "Visibility", true,
00253       "Changes the visibility of other Displays in the camera view.");
00254 
00255   visibility_property_->setIcon( loadPixmap("package://rviz/icons/visibility.svg",true) );
00256 
00257   this->addChild( visibility_property_, 0 );
00258   initializedp_ = true;
00259 }
00260 
00261 void OverlayCameraDisplay::preRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
00262 {
00263   QString image_position = image_position_property_->getString();
00264   bg_scene_node_->setVisible( caminfo_ok_ && (image_position == BACKGROUND || image_position == BOTH) );
00265   fg_scene_node_->setVisible( caminfo_ok_ && (image_position == OVERLAY || image_position == BOTH) );
00266 
00267   // set view flags on all displays
00268   visibility_property_->update();
00269 }
00270 
00271 void OverlayCameraDisplay::postRenderTargetUpdate(const Ogre::RenderTargetEvent& evt)
00272 {
00273   bg_scene_node_->setVisible( false );
00274   fg_scene_node_->setVisible( false );
00275 }
00276 
00277 void OverlayCameraDisplay::onEnable()
00278 {
00279   subscribe();
00280   render_panel_->getRenderWindow()->setActive(true);
00281   if (overlay_) {
00282     overlay_->show();
00283   }
00284 }
00285 
00286 void OverlayCameraDisplay::onDisable()
00287 {
00288   render_panel_->getRenderWindow()->setActive(false);
00289   unsubscribe();
00290   clear();
00291   if (overlay_) {
00292     overlay_->hide();
00293   }
00294 }
00295 
00296 void OverlayCameraDisplay::subscribe()
00297 {
00298   if ( (!isEnabled()) || (topic_property_->getTopicStd().empty()) )
00299   {
00300     return;
00301   }
00302 
00303   std::string target_frame = fixed_frame_.toStdString();
00304   ImageDisplayBase::enableTFFilter(target_frame);
00305 
00306   ImageDisplayBase::subscribe();
00307 
00308   std::string topic = topic_property_->getTopicStd();
00309   std::string caminfo_topic = image_transport::getCameraInfoTopic(topic_property_->getTopicStd());
00310 
00311   try
00312   {
00313     caminfo_sub_.subscribe( update_nh_, caminfo_topic, 1 );
00314     setStatus( StatusProperty::Ok, "Camera Info", "OK" );
00315   }
00316   catch( ros::Exception& e )
00317   {
00318     setStatus( StatusProperty::Error, "Camera Info", QString( "Error subscribing: ") + e.what() );
00319   }
00320 }
00321 
00322 void OverlayCameraDisplay::unsubscribe()
00323 {
00324   ImageDisplayBase::unsubscribe();
00325   caminfo_sub_.unsubscribe();
00326 }
00327 
00328 void OverlayCameraDisplay::updateAlpha()
00329 {
00330   float alpha = alpha_property_->getFloat();
00331 
00332   Ogre::Pass* pass = fg_material_->getTechnique( 0 )->getPass( 0 );
00333   if( pass->getNumTextureUnitStates() > 0 )
00334   {
00335     Ogre::TextureUnitState* tex_unit = pass->getTextureUnitState( 0 );
00336     tex_unit->setAlphaOperation( Ogre::LBX_MODULATE, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha );
00337   }
00338   else
00339   {
00340     fg_material_->setAmbient( Ogre::ColourValue( 0.0f, 1.0f, 1.0f, alpha ));
00341     fg_material_->setDiffuse( Ogre::ColourValue( 0.0f, 1.0f, 1.0f, alpha ));
00342   }
00343 
00344   force_render_ = true;
00345   context_->queueRender();
00346 }
00347 
00348 void OverlayCameraDisplay::forceRender()
00349 {
00350   force_render_ = true;
00351   context_->queueRender();
00352 }
00353 
00354 void OverlayCameraDisplay::updateQueueSize()
00355 {
00356   caminfo_tf_filter_->setQueueSize( (uint32_t) queue_size_property_->getInt() );
00357   ImageDisplayBase::updateQueueSize();
00358 }
00359 
00360 void OverlayCameraDisplay::clear()
00361 {
00362   texture_.clear();
00363   force_render_ = true;
00364   context_->queueRender();
00365 
00366   new_caminfo_ = false;
00367   current_caminfo_.reset();
00368 
00369   setStatus( StatusProperty::Warn, "Camera Info",
00370              "No CameraInfo received on [" + QString::fromStdString( caminfo_sub_.getTopic() ) + "].  Topic may not exist.");
00371   setStatus( StatusProperty::Warn, "Image", "No Image received");
00372 
00373   render_panel_->getCamera()->setPosition( Ogre::Vector3( 999999, 999999, 999999 ));
00374 }
00375 
00376 void OverlayCameraDisplay::update( float wall_dt, float ros_dt )
00377 {
00378   try
00379   {
00380     if( texture_.update() || force_render_ )
00381     {
00382       caminfo_ok_ = updateCamera();
00383       force_render_ = false;
00384     }
00385   }
00386   catch( UnsupportedImageEncoding& e )
00387   {
00388     setStatus( StatusProperty::Error, "Image", e.what() );
00389   }
00390 
00391   render_panel_->getRenderWindow()->update();
00392   if (!overlay_) {
00393     static int count = 0;
00394     rviz::UniformStringStream ss;
00395     ss << "OverlayImageDisplayObject" << count++;
00396     overlay_.reset(new OverlayObject(ss.str()));
00397     overlay_->show();
00398   }
00399   overlay_->updateTextureSize(render_panel_->getRenderWindow()->getWidth(),
00400                               render_panel_->getRenderWindow()->getHeight());
00401   redraw();
00402   overlay_->setDimensions(width_, height_);
00403   overlay_->setPosition(left_, top_);
00404 }
00405 
00406 void OverlayCameraDisplay::redraw()
00407 {
00408   Ogre::RenderTarget *rt = render_panel_->getRenderWindow();
00409   int width = rt->getWidth();
00410   int height = rt->getHeight();
00411   Ogre::uchar *data = new Ogre::uchar[width * height * 3];
00412   Ogre::PixelBox pb(width, height, 1, Ogre::PF_BYTE_RGB, data);
00413   rt->copyContentsToMemory(pb);
00414   {
00415     ScopedPixelBuffer buffer = overlay_->getBuffer();
00416     QImage Hud = buffer.getQImage(*overlay_);
00417     for (int i = 0; i < overlay_->getTextureWidth(); i++) {
00418       for (int j = 0; j < overlay_->getTextureHeight(); j++) {
00419         Ogre::ColourValue c = pb.getColourAt(i, j, 0);
00420         QColor color(c.r * 255, c.g * 255, c.b * 255, texture_alpha_ * 255);
00421         Hud.setPixel(i, j, color.rgba());
00422       }
00423     }
00424   }
00425   delete[] data;
00426 }
00427   
00428 bool OverlayCameraDisplay::updateCamera()
00429 {
00430   sensor_msgs::CameraInfo::ConstPtr info;
00431   sensor_msgs::Image::ConstPtr image;
00432   {
00433     boost::mutex::scoped_lock lock( caminfo_mutex_ );
00434 
00435     info = current_caminfo_;
00436     image = texture_.getImage();
00437   }
00438 
00439   if( !info || !image )
00440   {
00441     return false;
00442   }
00443 
00444   if( !validateFloats( *info ))
00445   {
00446     setStatus( StatusProperty::Error, "Camera Info", "Contains invalid floating point values (nans or infs)" );
00447     return false;
00448   }
00449 
00450   // if we're in 'exact' time mode, only show image if the time is exactly right
00451   ros::Time rviz_time = context_->getFrameManager()->getTime();
00452   if ( context_->getFrameManager()->getSyncMode() == FrameManager::SyncExact &&
00453       rviz_time != image->header.stamp )
00454   {
00455     std::ostringstream s;
00456     s << "Time-syncing active and no image at timestamp " << rviz_time.toSec() << ".";
00457     setStatus( StatusProperty::Warn, "Time", s.str().c_str() );
00458     return false;
00459   }
00460 
00461   Ogre::Vector3 position;
00462   Ogre::Quaternion orientation;
00463   context_->getFrameManager()->getTransform( image->header.frame_id, image->header.stamp, position, orientation );
00464 
00465   //printf( "OverlayCameraDisplay:updateCamera(): pos = %.2f, %.2f, %.2f.\n", position.x, position.y, position.z );
00466 
00467   // convert vision (Z-forward) frame to ogre frame (Z-out)
00468   orientation = orientation * Ogre::Quaternion( Ogre::Degree( 180 ), Ogre::Vector3::UNIT_X );
00469 
00470   float img_width = info->width;
00471   float img_height = info->height;
00472 
00473   // If the image width is 0 due to a malformed caminfo, try to grab the width from the image.
00474   if( img_width == 0 )
00475   {
00476     ROS_DEBUG( "Malformed CameraInfo on camera [%s], width = 0", qPrintable( getName() ));
00477     img_width = texture_.getWidth();
00478   }
00479 
00480   if (img_height == 0)
00481   {
00482     ROS_DEBUG( "Malformed CameraInfo on camera [%s], height = 0", qPrintable( getName() ));
00483     img_height = texture_.getHeight();
00484   }
00485 
00486   if( img_height == 0.0 || img_width == 0.0 )
00487   {
00488     setStatus( StatusProperty::Error, "Camera Info",
00489                "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)" );
00490     return false;
00491   }
00492 
00493   double fx = info->P[0];
00494   double fy = info->P[5];
00495 
00496   float win_width = render_panel_->width();
00497   float win_height = render_panel_->height();
00498   float zoom_x = zoom_property_->getFloat();
00499   float zoom_y = zoom_x;
00500 
00501   // Preserve aspect ratio
00502   if( win_width != 0 && win_height != 0 )
00503   {
00504     float img_aspect = (img_width/fx) / (img_height/fy);
00505     float win_aspect = win_width / win_height;
00506 
00507     if ( img_aspect > win_aspect )
00508     {
00509       zoom_y = zoom_y / img_aspect * win_aspect;
00510     }
00511     else
00512     {
00513       zoom_x = zoom_x / win_aspect * img_aspect;
00514     }
00515   }
00516 
00517   // Add the camera's translation relative to the left camera (from P[3]);
00518   double tx = -1 * (info->P[3] / fx);
00519   Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
00520   position = position + (right * tx);
00521 
00522   double ty = -1 * (info->P[7] / fy);
00523   Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y;
00524   position = position + (down * ty);
00525 
00526   if( !rviz::validateFloats( position ))
00527   {
00528     setStatus( StatusProperty::Error, "Camera Info", "CameraInfo/P resulted in an invalid position calculation (nans or infs)" );
00529     return false;
00530   }
00531 
00532   render_panel_->getCamera()->setPosition( position );
00533   render_panel_->getCamera()->setOrientation( orientation );
00534 
00535   // calculate the projection matrix
00536   double cx = info->P[2];
00537   double cy = info->P[6];
00538 
00539   double far_plane = 100;
00540   double near_plane = 0.01;
00541 
00542   Ogre::Matrix4 proj_matrix;
00543   proj_matrix = Ogre::Matrix4::ZERO;
00544  
00545   proj_matrix[0][0]= 2.0 * fx/img_width * zoom_x;
00546   proj_matrix[1][1]= 2.0 * fy/img_height * zoom_y;
00547 
00548   proj_matrix[0][2]= 2.0 * (0.5 - cx/img_width) * zoom_x;
00549   proj_matrix[1][2]= 2.0 * (cy/img_height - 0.5) * zoom_y;
00550 
00551   proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane);
00552   proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane);
00553 
00554   proj_matrix[3][2]= -1;
00555 
00556   render_panel_->getCamera()->setCustomProjectionMatrix( true, proj_matrix );
00557   render_panel_->resize(info->width, info->height);
00558   setStatus( StatusProperty::Ok, "Camera Info", "OK" );
00559 
00560 #if 0
00561   static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01);
00562   debug_axes->setPosition(position);
00563   debug_axes->setOrientation(orientation);
00564 #endif
00565 
00566   //adjust the image rectangles to fit the zoom & aspect ratio
00567   bg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y );
00568   fg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y );
00569 
00570   Ogre::AxisAlignedBox aabInf;
00571   aabInf.setInfinite();
00572   bg_screen_rect_->setBoundingBox( aabInf );
00573   fg_screen_rect_->setBoundingBox( aabInf );
00574 
00575   setStatus( StatusProperty::Ok, "Time", "ok" );
00576   setStatus( StatusProperty::Ok, "Camera Info", "ok" );
00577 
00578   return true;
00579 }
00580 
00581 void OverlayCameraDisplay::processMessage(const sensor_msgs::Image::ConstPtr& msg)
00582 {
00583   texture_.addMessage(msg);
00584 }
00585 
00586 void OverlayCameraDisplay::caminfoCallback( const sensor_msgs::CameraInfo::ConstPtr& msg )
00587 {
00588   boost::mutex::scoped_lock lock( caminfo_mutex_ );
00589   current_caminfo_ = msg;
00590   new_caminfo_ = true;
00591 }
00592 
00593 void OverlayCameraDisplay::fixedFrameChanged()
00594 {
00595   std::string targetFrame = fixed_frame_.toStdString();
00596   caminfo_tf_filter_->setTargetFrame(targetFrame);
00597   ImageDisplayBase::fixedFrameChanged();
00598 }
00599 
00600 void OverlayCameraDisplay::reset()
00601 {
00602   ImageDisplayBase::reset();
00603   clear();
00604 }
00605 
00606 void OverlayCameraDisplay::updateWidth()
00607 {
00608   width_ = width_property_->getInt();
00609 }
00610 
00611 void OverlayCameraDisplay::updateHeight()
00612 {
00613   height_ = height_property_->getInt();
00614 }
00615 
00616 void OverlayCameraDisplay::updateLeft()
00617 {
00618   left_ = left_property_->getInt();
00619 }
00620 
00621 void OverlayCameraDisplay::updateTop()
00622 {
00623   top_ = top_property_->getInt();
00624 }
00625 
00626 void OverlayCameraDisplay::updateTextureAlpha()
00627 {
00628   texture_alpha_ = texture_alpha_property_->getFloat();
00629 }
00630   
00631 }
00632 
00633 #include <pluginlib/class_list_macros.h>
00634 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::OverlayCameraDisplay, rviz::Display )


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03