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


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