visualization_manager.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, 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 <algorithm>
00031 
00032 #include <QApplication>
00033 #include <QCursor>
00034 #include <QPixmap>
00035 #include <QTimer>
00036 
00037 #include <boost/bind.hpp>
00038 
00039 #include <OGRE/OgreRoot.h>
00040 #include <OGRE/OgreSceneManager.h>
00041 #include <OGRE/OgreSceneNode.h>
00042 #include <OGRE/OgreLight.h>
00043 #include <OGRE/OgreViewport.h>
00044 #include <OGRE/OgreMaterialManager.h>
00045 #include <OGRE/OgreMaterial.h>
00046 #include <OGRE/OgreRenderWindow.h>
00047 
00048 #include <boost/filesystem.hpp>
00049 
00050 #include <tf/transform_listener.h>
00051 
00052 #include <ros/package.h>
00053 #include <ros/callback_queue.h>
00054 
00055 #include "rviz/display.h"
00056 #include "rviz/display_factory.h"
00057 #include "rviz/display_group.h"
00058 #include "rviz/displays_panel.h"
00059 #include "rviz/frame_manager.h"
00060 #include "rviz/ogre_helpers/qt_ogre_render_window.h"
00061 #include "rviz/properties/color_property.h"
00062 #include "rviz/properties/parse_color.h"
00063 #include "rviz/properties/property.h"
00064 #include "rviz/properties/property_tree_model.h"
00065 #include "rviz/properties/status_list.h"
00066 #include "rviz/properties/tf_frame_property.h"
00067 #include "rviz/render_panel.h"
00068 #include "rviz/selection/selection_manager.h"
00069 #include "rviz/tool.h"
00070 #include "rviz/tool_manager.h"
00071 #include "rviz/viewport_mouse_event.h"
00072 #include "rviz/view_controller.h"
00073 #include "rviz/view_manager.h"
00074 #include "rviz/load_resource.h"
00075 #include "rviz/ogre_helpers/ogre_render_queue_clearer.h"
00076 
00077 #include "rviz/visualization_manager.h"
00078 #include "rviz/window_manager_interface.h"
00079 
00080 namespace rviz
00081 {
00082 
00083 //helper class needed to display an icon besides "Global Options"
00084 class IconizedProperty: public Property {
00085 public:
00086   IconizedProperty( const QString& name = QString(),
00087               const QVariant default_value = QVariant(),
00088               const QString& description = QString(),
00089               Property* parent = 0,
00090               const char *changed_slot = 0,
00091               QObject* receiver = 0 )
00092   :Property( name, default_value, description, parent, changed_slot, receiver ) {};
00093   virtual QVariant getViewData( int column, int role ) const
00094   {
00095     return (column == 0 && role == Qt::DecorationRole)
00096         ? icon_ : Property::getViewData(column,role);
00097   }
00098   void setIcon( QIcon icon ) { icon_=icon; }
00099 private:
00100   QIcon icon_;
00101 };
00102 
00103 class VisualizationManagerPrivate
00104 {
00105 public:
00106   ros::CallbackQueue threaded_queue_;
00107   boost::thread_group threaded_queue_threads_;
00108   ros::NodeHandle update_nh_;
00109   ros::NodeHandle threaded_nh_;
00110   boost::mutex render_mutex_;
00111 };
00112 
00113 VisualizationManager::VisualizationManager( RenderPanel* render_panel, WindowManagerInterface* wm )
00114 : ogre_root_( Ogre::Root::getSingletonPtr() )
00115 , update_timer_(0)
00116 , shutting_down_(false)
00117 , render_panel_( render_panel )
00118 , time_update_timer_(0.0f)
00119 , frame_update_timer_(0.0f)
00120 , render_requested_(1)
00121 , frame_count_(0)
00122 , window_manager_(wm)
00123 , private_( new VisualizationManagerPrivate )
00124 , default_visibility_bit_( visibility_bit_allocator_.allocBit() )
00125 {
00126   frame_manager_ = new FrameManager();
00127 
00128   render_panel->setAutoRender(false);
00129 
00130   private_->threaded_nh_.setCallbackQueue(&private_->threaded_queue_);
00131 
00132   scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );
00133 
00134   directional_light_ = scene_manager_->createLight( "MainDirectional" );
00135   directional_light_->setType( Ogre::Light::LT_DIRECTIONAL );
00136   directional_light_->setDirection( Ogre::Vector3( -1, 0, -1 ) );
00137   directional_light_->setDiffuseColour( Ogre::ColourValue( 1.0f, 1.0f, 1.0f ) );
00138 
00139   root_display_group_ = new DisplayGroup();
00140   root_display_group_->setName( "root" );
00141   display_property_tree_model_ = new PropertyTreeModel( root_display_group_ );
00142   display_property_tree_model_->setDragDropClass( "display" );
00143   connect( display_property_tree_model_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));
00144   
00145   tool_manager_ = new ToolManager( this );
00146   connect( tool_manager_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));
00147   connect( tool_manager_, SIGNAL( toolChanged( Tool* ) ), this, SLOT( onToolChanged( Tool* ) ));
00148 
00149   view_manager_ = new ViewManager( this );
00150   view_manager_->setRenderPanel( render_panel_ );
00151   connect( view_manager_, SIGNAL( configChanged() ), this, SIGNAL( configChanged() ));
00152 
00153   IconizedProperty* ip = new IconizedProperty( "Global Options", QVariant(), "", root_display_group_ );
00154   ip->setIcon( loadPixmap("package://rviz/icons/options.png") );
00155   global_options_ = ip;
00156 
00157   fixed_frame_property_ = new TfFrameProperty( "Fixed Frame", "/map",
00158                                                "Frame into which all data is transformed before being displayed.",
00159                                                global_options_, frame_manager_, false,
00160                                                SLOT( updateFixedFrame() ), this );
00161 
00162   background_color_property_ = new ColorProperty( "Background Color", QColor(48,48,48),
00163                                                   "Background color for the 3D view.",
00164                                                   global_options_, SLOT( updateBackgroundColor() ), this );
00165 
00166   root_display_group_->initialize( this ); // only initialize() a Display after its sub-properties are created.
00167   root_display_group_->setEnabled( true );
00168 
00169   updateFixedFrame();
00170   updateBackgroundColor();
00171 
00172   global_status_ = new StatusList( "Global Status", root_display_group_ );
00173 
00174   createColorMaterials();
00175 
00176   selection_manager_ = new SelectionManager(this);
00177 
00178   private_->threaded_queue_threads_.create_thread(boost::bind(&VisualizationManager::threadedQueueThreadFunc, this));
00179 
00180   display_factory_ = new DisplayFactory();
00181 
00182   ogre_render_queue_clearer_ = new OgreRenderQueueClearer();
00183   Ogre::Root::getSingletonPtr()->addFrameListener( ogre_render_queue_clearer_ );
00184 
00185   update_timer_ = new QTimer;
00186   connect( update_timer_, SIGNAL( timeout() ), this, SLOT( onUpdate() ));
00187 }
00188 
00189 VisualizationManager::~VisualizationManager()
00190 {
00191   delete update_timer_;
00192 
00193   shutting_down_ = true;
00194   private_->threaded_queue_threads_.join_all();
00195 
00196   if(selection_manager_)
00197   {
00198     selection_manager_->setSelection(M_Picked());
00199   }
00200 
00201   delete display_property_tree_model_;
00202   delete tool_manager_;
00203   delete display_factory_;
00204   delete selection_manager_;
00205 
00206   if(ogre_root_)
00207   {
00208     ogre_root_->destroySceneManager( scene_manager_ );
00209   }
00210   delete frame_manager_;
00211   delete private_;
00212 
00213   Ogre::Root::getSingletonPtr()->removeFrameListener( ogre_render_queue_clearer_ );
00214   delete ogre_render_queue_clearer_;
00215 }
00216 
00217 void VisualizationManager::initialize()
00218 {
00219   emitStatusUpdate( "Initializing managers." );
00220 
00221   view_manager_->initialize();
00222   selection_manager_->initialize();
00223   tool_manager_->initialize();
00224 
00225   last_update_ros_time_ = ros::Time::now();
00226   last_update_wall_time_ = ros::WallTime::now();
00227 }
00228 
00229 ros::CallbackQueueInterface* VisualizationManager::getThreadedQueue()
00230 {
00231   return &private_->threaded_queue_;
00232 }
00233 
00234 void VisualizationManager::lockRender()
00235 {
00236   private_->render_mutex_.lock();
00237 }
00238 
00239 void VisualizationManager::unlockRender()
00240 {
00241   private_->render_mutex_.unlock();
00242 }
00243 
00244 ros::CallbackQueueInterface* VisualizationManager::getUpdateQueue()
00245 {
00246   return ros::getGlobalCallbackQueue();
00247 }
00248 
00249 void VisualizationManager::startUpdate()
00250 {
00251   update_timer_->start( 33 );
00252 }
00253 
00254 void VisualizationManager::stopUpdate()
00255 {
00256   update_timer_->stop();
00257 }
00258 
00259 void createColorMaterial(const std::string& name, const Ogre::ColourValue& color, bool use_self_illumination)
00260 {
00261   Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create( name, ROS_PACKAGE_NAME );
00262   mat->setAmbient(color * 0.5f);
00263   mat->setDiffuse(color);
00264   if( use_self_illumination )
00265   {
00266     mat->setSelfIllumination(color);
00267   }
00268   mat->setLightingEnabled(true);
00269   mat->setReceiveShadows(false);
00270 }
00271 
00272 void VisualizationManager::createColorMaterials()
00273 {
00274   createColorMaterial("RVIZ/Red", Ogre::ColourValue(1.0f, 0.0f, 0.0f, 1.0f), true);
00275   createColorMaterial("RVIZ/Green", Ogre::ColourValue(0.0f, 1.0f, 0.0f, 1.0f), true);
00276   createColorMaterial("RVIZ/Blue", Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f), true);
00277   createColorMaterial("RVIZ/Cyan", Ogre::ColourValue(0.0f, 1.0f, 1.0f, 1.0f), true);
00278   createColorMaterial("RVIZ/ShadedRed", Ogre::ColourValue(1.0f, 0.0f, 0.0f, 1.0f), false);
00279   createColorMaterial("RVIZ/ShadedGreen", Ogre::ColourValue(0.0f, 1.0f, 0.0f, 1.0f), false);
00280   createColorMaterial("RVIZ/ShadedBlue", Ogre::ColourValue(0.0f, 0.0f, 1.0f, 1.0f), false);
00281   createColorMaterial("RVIZ/ShadedCyan", Ogre::ColourValue(0.0f, 1.0f, 1.0f, 1.0f), false);
00282 }
00283 
00284 void VisualizationManager::queueRender()
00285 {
00286   render_requested_ = 1;
00287 }
00288 
00289 void VisualizationManager::onUpdate()
00290 {
00291   ros::WallDuration wall_diff = ros::WallTime::now() - last_update_wall_time_;
00292   ros::Duration ros_diff = ros::Time::now() - last_update_ros_time_;
00293   float wall_dt = wall_diff.toSec();
00294   float ros_dt = ros_diff.toSec();
00295   last_update_ros_time_ = ros::Time::now();
00296   last_update_wall_time_ = ros::WallTime::now();
00297 
00298   if(ros_dt < 0.0)
00299   {
00300     resetTime();
00301   }
00302 
00303   ros::spinOnce();
00304 
00305   Q_EMIT preUpdate();
00306 
00307   frame_manager_->update();
00308 
00309   root_display_group_->update( wall_dt, ros_dt );
00310 
00311   view_manager_->update(wall_dt, ros_dt);
00312 
00313   time_update_timer_ += wall_dt;
00314 
00315   if( time_update_timer_ > 0.1f )
00316   {
00317     time_update_timer_ = 0.0f;
00318 
00319     updateTime();
00320   }
00321 
00322   frame_update_timer_ += wall_dt;
00323 
00324   if(frame_update_timer_ > 1.0f)
00325   {
00326     frame_update_timer_ = 0.0f;
00327 
00328     updateFrames();
00329   }
00330 
00331   selection_manager_->update();
00332 
00333   if( tool_manager_->getCurrentTool() )
00334   {
00335     tool_manager_->getCurrentTool()->update(wall_dt, ros_dt);
00336   }
00337 
00338   if ( view_manager_ &&
00339         view_manager_->getCurrent() &&
00340         view_manager_->getCurrent()->getCamera() )
00341   {
00342     directional_light_->setDirection(view_manager_->getCurrent()->getCamera()->getDerivedDirection());
00343   }
00344 
00345   frame_count_++;
00346 
00347   if ( render_requested_ || wall_dt > 0.01 )
00348   {
00349     render_requested_ = 0;
00350     boost::mutex::scoped_lock lock(private_->render_mutex_);
00351     ogre_root_->renderOneFrame();
00352   }
00353 }
00354 
00355 void VisualizationManager::updateTime()
00356 {
00357   if( ros_time_begin_.isZero() )
00358   {
00359     ros_time_begin_ = ros::Time::now();
00360   }
00361 
00362   ros_time_elapsed_ = ros::Time::now() - ros_time_begin_;
00363 
00364   if( wall_clock_begin_.isZero() )
00365   {
00366     wall_clock_begin_ = ros::WallTime::now();
00367   }
00368 
00369   wall_clock_elapsed_ = ros::WallTime::now() - wall_clock_begin_;
00370 }
00371 
00372 void VisualizationManager::updateFrames()
00373 {
00374   typedef std::vector<std::string> V_string;
00375   V_string frames;
00376   frame_manager_->getTFClient()->getFrameStrings( frames );
00377 
00378   // Check the fixed frame to see if it's ok
00379   std::string error;
00380   if( frame_manager_->frameHasProblems( getFixedFrame().toStdString(), ros::Time(), error ))
00381   {
00382     if( frames.empty() )
00383     {
00384       // fixed_prop->setToWarn();
00385       std::stringstream ss;
00386       ss << "No tf data.  Actual error: " << error;
00387       global_status_->setStatus( StatusProperty::Warn, "Fixed Frame", QString::fromStdString( ss.str() ));
00388     }
00389     else
00390     {
00391       // fixed_prop->setToError();
00392       global_status_->setStatus( StatusProperty::Error, "Fixed Frame", QString::fromStdString( error ));
00393     }
00394   }
00395   else
00396   {
00397     // fixed_prop->setToOK();
00398     global_status_->setStatus( StatusProperty::Ok, "Fixed Frame", "OK" );
00399   }
00400 }
00401 
00402 tf::TransformListener* VisualizationManager::getTFClient() const
00403 {
00404   return frame_manager_->getTFClient();
00405 }
00406 
00407 void VisualizationManager::resetTime()
00408 {
00409   root_display_group_->reset();
00410   frame_manager_->getTFClient()->clear();
00411 
00412   ros_time_begin_ = ros::Time();
00413   wall_clock_begin_ = ros::WallTime();
00414 
00415   queueRender();
00416 }
00417 
00418 void VisualizationManager::addDisplay( Display* display, bool enabled )
00419 {
00420   root_display_group_->addDisplay( display );
00421   display->initialize( this );
00422   display->setEnabled( enabled );
00423 }
00424 
00425 void VisualizationManager::removeAllDisplays()
00426 {
00427   root_display_group_->removeAllDisplays();
00428 }
00429 
00430 void VisualizationManager::emitStatusUpdate( const QString& message )
00431 {
00432   Q_EMIT statusUpdate( message );
00433 }
00434 
00435 void VisualizationManager::load( const Config& config )
00436 {
00437   stopUpdate();
00438 
00439   emitStatusUpdate( "Creating displays" );
00440   root_display_group_->load( config );
00441 
00442   emitStatusUpdate( "Creating tools" );
00443   tool_manager_->load( config.mapGetChild( "Tools" ));
00444 
00445   emitStatusUpdate( "Creating views" );
00446   view_manager_->load( config.mapGetChild( "Views" ));
00447 
00448   startUpdate();
00449 }
00450 
00451 void VisualizationManager::save( Config config ) const
00452 {
00453   root_display_group_->save( config );
00454   tool_manager_->save( config.mapMakeChild( "Tools" ));
00455   view_manager_->save( config.mapMakeChild( "Views" ));
00456 }
00457 
00458 Display* VisualizationManager::createDisplay( const QString& class_lookup_name,
00459                                               const QString& name,
00460                                               bool enabled )
00461 {
00462   QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
00463   Display* new_display = root_display_group_->createDisplay( class_lookup_name );
00464   addDisplay( new_display, enabled );
00465   new_display->setName( name );
00466   QApplication::restoreOverrideCursor();
00467   return new_display;
00468 }
00469 
00470 double VisualizationManager::getWallClock()
00471 {
00472   return ros::WallTime::now().toSec();
00473 }
00474 
00475 double VisualizationManager::getROSTime()
00476 {
00477   return frame_manager_->getTime().toSec();
00478 }
00479 
00480 double VisualizationManager::getWallClockElapsed()
00481 {
00482   return wall_clock_elapsed_.toSec();
00483 }
00484 
00485 double VisualizationManager::getROSTimeElapsed()
00486 {
00487   return (frame_manager_->getTime() - ros_time_begin_).toSec();
00488 }
00489 
00490 void VisualizationManager::updateBackgroundColor()
00491 {
00492   render_panel_->setBackgroundColor( qtToOgre( background_color_property_->getColor() ));
00493 
00494   queueRender();
00495 }
00496 
00497 void VisualizationManager::handleMouseEvent( const ViewportMouseEvent& vme )
00498 {
00499   //process pending mouse events
00500   Tool* current_tool = tool_manager_->getCurrentTool();
00501 
00502   int flags = 0;
00503   if( current_tool )
00504   {
00505     ViewportMouseEvent _vme = vme;
00506     flags = current_tool->processMouseEvent( _vme );
00507     vme.panel->setCursor( current_tool->getCursor() );
00508   }
00509   else
00510   {
00511     vme.panel->setCursor( QCursor( Qt::ArrowCursor ) );
00512   }
00513 
00514   if( flags & Tool::Render )
00515   {
00516     queueRender();
00517   }
00518 
00519   if( flags & Tool::Finished )
00520   {
00521     tool_manager_->setCurrentTool( tool_manager_->getDefaultTool() );
00522   }
00523 }
00524 
00525 void VisualizationManager::handleChar( QKeyEvent* event, RenderPanel* panel )
00526 {
00527   tool_manager_->handleChar( event, panel );
00528 }
00529 
00530 void VisualizationManager::threadedQueueThreadFunc()
00531 {
00532   while (!shutting_down_)
00533   {
00534     private_->threaded_queue_.callOne(ros::WallDuration(0.1));
00535   }
00536 }
00537 
00538 void VisualizationManager::notifyConfigChanged()
00539 {
00540   Q_EMIT configChanged();
00541 }
00542 
00543 void VisualizationManager::onToolChanged( Tool* tool )
00544 {
00545 }
00546 
00547 void VisualizationManager::updateFixedFrame()
00548 {
00549   QString frame = fixed_frame_property_->getFrame();
00550 
00551   frame_manager_->setFixedFrame( frame.toStdString() );
00552   root_display_group_->setFixedFrame( frame );
00553 }
00554 
00555 QString VisualizationManager::getFixedFrame() const
00556 {
00557   return fixed_frame_property_->getFrame();
00558 }
00559 
00560 void VisualizationManager::setFixedFrame( const QString& frame )
00561 {
00562   fixed_frame_property_->setValue( frame );
00563 }
00564 
00565 void VisualizationManager::setStatus( const QString & message )
00566 {
00567   emitStatusUpdate( message );
00568 }
00569 
00570 } // namespace rviz


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Oct 6 2014 07:26:36