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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:31