visualization_frame.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, 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 <fstream>
00031 
00032 #include <QAction>
00033 #include <QApplication>
00034 #include <QCloseEvent>
00035 #include <QDesktopServices>
00036 #include <QDockWidget>
00037 #include <QDir>
00038 #include <QFileDialog>
00039 #include <QMenu>
00040 #include <QMenuBar>
00041 #include <QMessageBox>
00042 #include <QTimer>
00043 #include <QToolBar>
00044 #include <QToolButton>
00045 #include <QUrl>
00046 #include <QStatusBar>
00047 #include <QLabel>
00048 #include <QToolButton>
00049 #include <QHBoxLayout>
00050 
00051 #include <boost/algorithm/string/split.hpp>
00052 #include <boost/algorithm/string/trim.hpp>
00053 #include <boost/bind.hpp>
00054 #include <boost/filesystem.hpp>
00055 
00056 #include <ros/console.h>
00057 #include <ros/package.h>
00058 #include <ros/init.h>
00059 
00060 #include <OgreRenderWindow.h>
00061 #include <OgreMeshManager.h>
00062 
00063 #include <ogre_helpers/initialization.h>
00064 
00065 #include "rviz/displays_panel.h"
00066 #include "rviz/failed_panel.h"
00067 #include "rviz/help_panel.h"
00068 #include "rviz/loading_dialog.h"
00069 #include "rviz/new_object_dialog.h"
00070 #include "rviz/panel_dock_widget.h"
00071 #include "rviz/panel_factory.h"
00072 #include "rviz/render_panel.h"
00073 #include "rviz/screenshot_dialog.h"
00074 #include "rviz/selection/selection_manager.h"
00075 #include "rviz/selection_panel.h"
00076 #include "rviz/splash_screen.h"
00077 #include "rviz/time_panel.h"
00078 #include "rviz/tool.h"
00079 #include "rviz/tool_manager.h"
00080 #include "rviz/tool_properties_panel.h"
00081 #include "rviz/views_panel.h"
00082 #include "rviz/visualization_manager.h"
00083 #include "rviz/widget_geometry_change_detector.h"
00084 #include "rviz/load_resource.h"
00085 #include "rviz/yaml_config_reader.h"
00086 #include "rviz/yaml_config_writer.h"
00087 
00088 #include "rviz/visualization_frame.h"
00089 
00090 namespace fs = boost::filesystem;
00091 
00092 #define CONFIG_EXTENSION "rviz"
00093 #define CONFIG_EXTENSION_WILDCARD "*."CONFIG_EXTENSION
00094 #define RECENT_CONFIG_COUNT 10
00095 
00096 #if BOOST_FILESYSTEM_VERSION == 3
00097 #define BOOST_FILENAME_STRING filename().string
00098 #define BOOST_FILE_STRING string
00099 #else
00100 #define BOOST_FILENAME_STRING filename
00101 #define BOOST_FILE_STRING file_string
00102 #endif
00103 
00104 namespace rviz
00105 {
00106 
00107 VisualizationFrame::VisualizationFrame( QWidget* parent )
00108   : QMainWindow( parent )
00109   , render_panel_(NULL)
00110   , show_help_action_(NULL)
00111   , file_menu_(NULL)
00112   , recent_configs_menu_(NULL)
00113   , toolbar_(NULL)
00114   , manager_(NULL)
00115   , splash_( NULL )
00116   , toolbar_actions_( NULL )
00117   , show_choose_new_master_option_( false )
00118   , add_tool_action_( NULL )
00119   , remove_tool_menu_( NULL )
00120   , initialized_( false )
00121   , geom_change_detector_( new WidgetGeometryChangeDetector( this ))
00122   , loading_( false )
00123   , post_load_timer_( new QTimer( this ))
00124   , frame_count_(0)
00125 {
00126   panel_factory_ = new PanelFactory();
00127 
00128   installEventFilter( geom_change_detector_ );
00129   connect( geom_change_detector_, SIGNAL( changed() ), this, SLOT( setDisplayConfigModified() ));
00130 
00131   post_load_timer_->setSingleShot( true );
00132   connect( post_load_timer_, SIGNAL( timeout() ), this, SLOT( markLoadingDone() ));
00133 
00134   package_path_ = ros::package::getPath("rviz");
00135   help_path_ = QString::fromStdString( (fs::path(package_path_) / "help/help.html").BOOST_FILE_STRING() );
00136   splash_path_ = QString::fromStdString( (fs::path(package_path_) / "images/splash.png").BOOST_FILE_STRING() );
00137 
00138   QToolButton* reset_button = new QToolButton( );
00139   reset_button->setText( "Reset" );
00140   reset_button->setContentsMargins(0,0,0,0);
00141   statusBar()->addPermanentWidget( reset_button, 0 );
00142   connect( reset_button, SIGNAL( clicked( bool )), this, SLOT( reset() ));
00143 
00144   status_label_ = new QLabel("");
00145   statusBar()->addPermanentWidget( status_label_, 1 );
00146   connect( this, SIGNAL( statusUpdate( const QString& )), status_label_, SLOT( setText( const QString& )));
00147 
00148   fps_label_ = new QLabel("");
00149   fps_label_->setMinimumWidth(40);
00150   fps_label_->setAlignment(Qt::AlignRight);
00151   statusBar()->addPermanentWidget( fps_label_, 0 );
00152 
00153   setWindowTitle( "RViz[*]" );
00154 }
00155 
00156 VisualizationFrame::~VisualizationFrame()
00157 {
00158   delete render_panel_;
00159   delete manager_;
00160 
00161   for( int i = 0; i < custom_panels_.size(); i++ )
00162   {
00163     delete custom_panels_[ i ].dock;
00164   }
00165 
00166   delete panel_factory_;
00167 }
00168 
00169 void VisualizationFrame::setStatus( const QString & message )
00170 {
00171   Q_EMIT statusUpdate( message );
00172 }
00173 
00174 void VisualizationFrame::updateFps()
00175 {
00176   frame_count_ ++;
00177   ros::WallDuration wall_diff = ros::WallTime::now() - last_fps_calc_time_;
00178 
00179   if ( wall_diff.toSec() > 1.0 )
00180   {
00181     float fps = frame_count_ / wall_diff.toSec();
00182     frame_count_ = 0;
00183     last_fps_calc_time_ = ros::WallTime::now();
00184     fps_label_->setText( QString::number(int(fps)) + QString(" fps") );
00185   }
00186 }
00187 
00188 void VisualizationFrame::closeEvent( QCloseEvent* event )
00189 {
00190   if( prepareToExit() )
00191   {
00192     event->accept();
00193   }
00194   else
00195   {
00196     event->ignore();
00197   }
00198 }
00199 
00200 void VisualizationFrame::leaveEvent ( QEvent * event )
00201 {
00202   setStatus("");
00203 }
00204 
00205 void VisualizationFrame::reset()
00206 {
00207   Ogre::MeshManager::getSingleton().removeAll();
00208   manager_->resetTime();
00209 }
00210 
00211 void VisualizationFrame::changeMaster()
00212 {
00213   if( prepareToExit() )
00214   {
00215     QApplication::exit( 255 );
00216   }
00217 }
00218 
00219 void VisualizationFrame::setShowChooseNewMaster( bool show )
00220 {
00221   show_choose_new_master_option_ = show;
00222 }
00223 
00224 void VisualizationFrame::setHelpPath( const QString& help_path )
00225 {
00226   help_path_ = help_path;
00227   manager_->setHelpPath( help_path_ );
00228 }
00229 
00230 void VisualizationFrame::setSplashPath( const QString& splash_path )
00231 {
00232   splash_path_ = splash_path;
00233 }
00234 
00235 void VisualizationFrame::initialize(const QString& display_config_file )
00236 {
00237   initConfigs();
00238 
00239   loadPersistentSettings();
00240 
00241   QIcon app_icon( QString::fromStdString( (fs::path(package_path_) / "icons/package.png").BOOST_FILE_STRING() ) );
00242   setWindowIcon( app_icon );
00243 
00244   if( splash_path_ != "" )
00245   {
00246     QPixmap splash_image( splash_path_ );
00247     splash_ = new SplashScreen( splash_image );
00248     splash_->show();
00249     connect( this, SIGNAL( statusUpdate( const QString& )), splash_, SLOT( showMessage( const QString& )));
00250   }
00251   Q_EMIT statusUpdate( "Initializing" );
00252 
00253   if( !ros::isInitialized() )
00254   {
00255     int argc = 0;
00256     ros::init( argc, 0, "rviz", ros::init_options::AnonymousName );
00257   }
00258 
00259   QWidget* central_widget = new QWidget(this);
00260   QHBoxLayout* central_layout = new QHBoxLayout;
00261   central_layout->setSpacing(0);
00262   central_layout->setMargin(0);
00263 
00264   render_panel_ = new RenderPanel( central_widget );
00265 
00266   hide_left_dock_button_ = new QToolButton();
00267   hide_left_dock_button_->setContentsMargins(0,0,0,0);
00268   hide_left_dock_button_->setArrowType( Qt::LeftArrow );
00269   hide_left_dock_button_->setSizePolicy( QSizePolicy( QSizePolicy::Minimum, QSizePolicy::Expanding ) );
00270   hide_left_dock_button_->setFixedWidth(16);
00271   hide_left_dock_button_->setAutoRaise(true);
00272   hide_left_dock_button_->setCheckable(true);
00273 
00274   connect(hide_left_dock_button_, SIGNAL(toggled(bool)), this, SLOT(hideLeftDock(bool)));
00275 
00276   hide_right_dock_button_ = new QToolButton();
00277   hide_right_dock_button_->setContentsMargins(0,0,0,0);
00278   hide_right_dock_button_->setArrowType( Qt::RightArrow );
00279   hide_right_dock_button_->setSizePolicy( QSizePolicy( QSizePolicy::Minimum, QSizePolicy::Expanding ) );
00280   hide_right_dock_button_->setFixedWidth(16);
00281   hide_right_dock_button_->setAutoRaise(true);
00282   hide_right_dock_button_->setCheckable(true);
00283 
00284   connect(hide_right_dock_button_, SIGNAL(toggled(bool)), this, SLOT(hideRightDock(bool)));
00285 
00286   central_layout->addWidget( hide_left_dock_button_, 0 );
00287   central_layout->addWidget( render_panel_, 1 );
00288   central_layout->addWidget( hide_right_dock_button_, 0 );
00289 
00290   central_widget->setLayout( central_layout );
00291 
00292   initMenus();
00293 
00294   initToolbars();
00295 
00296   setCentralWidget( central_widget );
00297 
00298   manager_ = new VisualizationManager( render_panel_, this );
00299   manager_->setHelpPath( help_path_ );
00300 
00301   render_panel_->initialize( manager_->getSceneManager(), manager_ );
00302 
00303   ToolManager* tool_man = manager_->getToolManager();
00304 
00305   connect( manager_, SIGNAL( configChanged() ), this, SLOT( setDisplayConfigModified() ));
00306   connect( tool_man, SIGNAL( toolAdded( Tool* )), this, SLOT( addTool( Tool* )));
00307   connect( tool_man, SIGNAL( toolRemoved( Tool* )), this, SLOT( removeTool( Tool* )));
00308   connect( tool_man, SIGNAL( toolChanged( Tool* )), this, SLOT( indicateToolIsCurrent( Tool* )));
00309 
00310   manager_->initialize();
00311 
00312   if( display_config_file != "" )
00313   {
00314     loadDisplayConfig( display_config_file );
00315   }
00316   else
00317   {
00318     loadDisplayConfig( QString::fromStdString( default_display_config_file_ ));
00319   }
00320 
00321   delete splash_;
00322   splash_ = 0;
00323 
00324   manager_->startUpdate();
00325   initialized_ = true;
00326   Q_EMIT statusUpdate( "RViz is ready." );
00327 
00328   connect( manager_, SIGNAL( preUpdate() ), this, SLOT( updateFps() ) );
00329   connect( manager_, SIGNAL( statusUpdate( const QString& )), this, SIGNAL( statusUpdate( const QString& )));
00330 }
00331 
00332 void VisualizationFrame::initConfigs()
00333 {
00334   home_dir_ = QDir::toNativeSeparators( QDir::homePath() ).toStdString();
00335 
00336   config_dir_ = (fs::path(home_dir_) / ".rviz").BOOST_FILE_STRING();
00337   persistent_settings_file_ = (fs::path(config_dir_) / "persistent_settings").BOOST_FILE_STRING();
00338   default_display_config_file_ = (fs::path(config_dir_) / "default."CONFIG_EXTENSION).BOOST_FILE_STRING();
00339 
00340   if( fs::is_regular_file( config_dir_ ))
00341   {
00342     ROS_ERROR("Moving file [%s] out of the way to recreate it as a directory.", config_dir_.c_str());
00343     std::string backup_file = config_dir_ + ".bak";
00344 
00345     fs::rename(config_dir_, backup_file);
00346     fs::create_directory(config_dir_);
00347   }
00348   else if (!fs::exists(config_dir_))
00349   {
00350     fs::create_directory(config_dir_);
00351   }
00352 }
00353 
00354 void VisualizationFrame::loadPersistentSettings()
00355 {
00356   YamlConfigReader reader;
00357   Config config;
00358   reader.readFile( config, QString::fromStdString( persistent_settings_file_ ));
00359   if( !reader.error() )
00360   {
00361     QString last_config_dir, last_image_dir;
00362     if( config.mapGetString( "Last Config Dir", &last_config_dir ) &&
00363         config.mapGetString( "Last Image Dir", &last_image_dir ))
00364     {
00365       last_config_dir_ = last_config_dir.toStdString();
00366       last_image_dir_ = last_image_dir.toStdString();
00367     }
00368     
00369     Config recent_configs_list = config.mapGetChild( "Recent Configs" );
00370     recent_configs_.clear();
00371     int num_recent = recent_configs_list.listLength();
00372     for( int i = 0; i < num_recent; i++ )
00373     {
00374       recent_configs_.push_back( recent_configs_list.listChildAt( i ).getValue().toString().toStdString() );
00375     }
00376   }
00377   else
00378   {
00379     ROS_ERROR( "%s", qPrintable( reader.errorMessage() ));
00380   }
00381 }
00382 
00383 void VisualizationFrame::savePersistentSettings()
00384 {
00385   Config config;
00386   config.mapSetValue( "Last Config Dir", QString::fromStdString( last_config_dir_ ));
00387   config.mapSetValue( "Last Image Dir", QString::fromStdString( last_image_dir_ ));
00388   Config recent_configs_list = config.mapMakeChild( "Recent Configs" );
00389   for( D_string::iterator it = recent_configs_.begin(); it != recent_configs_.end(); ++it )
00390   {
00391     recent_configs_list.listAppendNew().setValue( QString::fromStdString( *it ));
00392   }
00393 
00394   YamlConfigWriter writer;
00395   writer.writeFile( config, QString::fromStdString( persistent_settings_file_ ));
00396 
00397   if( writer.error() )
00398   {
00399     ROS_ERROR( "%s", qPrintable( writer.errorMessage() ));
00400   }
00401 }
00402 
00403 void VisualizationFrame::initMenus()
00404 {
00405   file_menu_ = menuBar()->addMenu( "&File" );
00406   file_menu_->addAction( "&Open Config", this, SLOT( onOpen() ), QKeySequence( "Ctrl+O" ));
00407   file_menu_->addAction( "&Save Config", this, SLOT( onSave() ), QKeySequence( "Ctrl+S" ));
00408   file_menu_->addAction( "Save Config &As", this, SLOT( onSaveAs() ));
00409   recent_configs_menu_ = file_menu_->addMenu( "&Recent Configs" );
00410   file_menu_->addAction( "Save &Image", this, SLOT( onSaveImage() ));
00411   if( show_choose_new_master_option_ )
00412   {
00413     file_menu_->addSeparator();
00414     file_menu_->addAction( "Change &Master", this, SLOT( changeMaster() ));
00415   }
00416   file_menu_->addSeparator();
00417   file_menu_->addAction( "&Quit", this, SLOT( close() ), QKeySequence( "Ctrl+Q" ));
00418 
00419   view_menu_ = menuBar()->addMenu( "&Panels" );
00420   view_menu_->addAction( "Add &New Panel", this, SLOT( openNewPanelDialog() ));
00421   delete_view_menu_ = view_menu_->addMenu( "&Delete Panel" );
00422   delete_view_menu_->setEnabled( false );
00423   view_menu_->addSeparator();
00424 
00425   QMenu* help_menu = menuBar()->addMenu( "&Help" );
00426   help_menu->addAction( "Show &Help panel", this, SLOT( showHelpPanel() ));
00427   help_menu->addAction( "Open rviz wiki in browser", this, SLOT( onHelpWiki() ));
00428 }
00429 
00430 void VisualizationFrame::initToolbars()
00431 {
00432   QFont font;
00433   font.setPointSize( font.pointSizeF()*0.9 );
00434 
00435   // make toolbar with plugin tools
00436 
00437   toolbar_ = addToolBar( "Tools" );
00438   toolbar_->setFont( font );
00439   toolbar_->setContentsMargins(0,0,0,0);
00440   toolbar_->setObjectName( "Tools" );
00441   toolbar_->setToolButtonStyle( Qt::ToolButtonTextBesideIcon );
00442   toolbar_actions_ = new QActionGroup( this );
00443   connect( toolbar_actions_, SIGNAL( triggered( QAction* )), this, SLOT( onToolbarActionTriggered( QAction* )));
00444   view_menu_->addAction( toolbar_->toggleViewAction() );
00445 
00446   add_tool_action_ = new QAction( "", toolbar_actions_ );
00447   add_tool_action_->setToolTip( "Add a new tool" );
00448   add_tool_action_->setIcon( loadPixmap( "package://rviz/icons/plus.png" ) );
00449   toolbar_->addAction( add_tool_action_ );
00450   connect( add_tool_action_, SIGNAL( triggered() ), this, SLOT( openNewToolDialog() ));
00451 
00452   remove_tool_menu_ = new QMenu();
00453   QToolButton* remove_tool_button = new QToolButton();
00454   remove_tool_button->setMenu( remove_tool_menu_ );
00455   remove_tool_button->setPopupMode( QToolButton::InstantPopup );
00456   remove_tool_button->setToolTip( "Remove a tool from the toolbar" );
00457   remove_tool_button->setIcon( loadPixmap( "package://rviz/icons/minus.png" ) );
00458   toolbar_->addWidget( remove_tool_button );
00459   connect( remove_tool_menu_, SIGNAL( triggered( QAction* )), this, SLOT( onToolbarRemoveTool( QAction* )));
00460 
00461 }
00462 
00463 void VisualizationFrame::hideDockImpl( Qt::DockWidgetArea area, bool hide )
00464 {
00465   QList<PanelDockWidget *> dock_widgets = findChildren<PanelDockWidget *>();
00466 
00467   for ( QList<PanelDockWidget *>::iterator it=dock_widgets.begin(); it!=dock_widgets.end(); it++ )
00468   {
00469     Qt::DockWidgetArea curr_area = dockWidgetArea ( *it );
00470     if ( area == curr_area )
00471     {
00472       (*it)->setCollapsed(hide);
00473     }
00474     // allow/disallow docking to this area for all widgets
00475     if ( hide )
00476     {
00477       (*it)->setAllowedAreas( (*it)->allowedAreas() & ~area );
00478     }
00479     else
00480     {
00481       (*it)->setAllowedAreas( (*it)->allowedAreas() | area );
00482     }
00483   }
00484 }
00485 
00486 void VisualizationFrame::setHideButtonVisibility( bool visible )
00487 {
00488   hide_left_dock_button_->setVisible( visible );
00489   hide_right_dock_button_->setVisible( visible );
00490 }
00491 
00492 void VisualizationFrame::hideLeftDock( bool hide )
00493 {
00494   hideDockImpl( Qt::LeftDockWidgetArea, hide );
00495   hide_left_dock_button_->setArrowType( hide ? Qt::RightArrow : Qt::LeftArrow );
00496 }
00497 
00498 void VisualizationFrame::hideRightDock( bool hide )
00499 {
00500   hideDockImpl( Qt::RightDockWidgetArea, hide );
00501   hide_right_dock_button_->setArrowType( hide ? Qt::LeftArrow : Qt::RightArrow );
00502 }
00503 
00504 void VisualizationFrame::onDockPanelVisibilityChange( bool visible )
00505 {
00506   // if a dock widget becomes visible and is resting inside the
00507   // left or right dock area, we want to unhide the whole area
00508   if ( visible )
00509   {
00510     QDockWidget* dock_widget = dynamic_cast<QDockWidget*>( sender() );
00511     if ( dock_widget )
00512     {
00513       Qt::DockWidgetArea area = dockWidgetArea( dock_widget );
00514       if ( area == Qt::LeftDockWidgetArea )
00515       {
00516         hide_left_dock_button_->setChecked( false );
00517       }
00518       if ( area == Qt::RightDockWidgetArea )
00519       {
00520         hide_right_dock_button_->setChecked( false );
00521       }
00522     }
00523   }
00524 
00525 }
00526 
00527 void VisualizationFrame::openNewPanelDialog()
00528 {
00529   QString class_id;
00530   QString display_name;
00531   QStringList empty;
00532 
00533   NewObjectDialog* dialog = new NewObjectDialog( panel_factory_,
00534                                                  "Panel",
00535                                                  empty,
00536                                                  empty,
00537                                                  &class_id,
00538                                                  &display_name,
00539                                                  this );
00540   manager_->stopUpdate();
00541   if( dialog->exec() == QDialog::Accepted )
00542   {
00543     addPanelByName( display_name, class_id );
00544   }
00545   manager_->startUpdate();
00546 }
00547 
00548 void VisualizationFrame::openNewToolDialog()
00549 {
00550   QString class_id;
00551   QStringList empty;
00552   ToolManager* tool_man = manager_->getToolManager();
00553 
00554   NewObjectDialog* dialog = new NewObjectDialog( tool_man->getFactory(),
00555                                                  "Tool",
00556                                                  empty,
00557                                                  tool_man->getToolClasses(),
00558                                                  &class_id );
00559   manager_->stopUpdate();
00560   if( dialog->exec() == QDialog::Accepted )
00561   {
00562     tool_man->addTool( class_id );
00563   }
00564   manager_->startUpdate();
00565   activateWindow(); // Force keyboard focus back on main window.
00566 }
00567 
00568 void VisualizationFrame::updateRecentConfigMenu()
00569 {
00570   recent_configs_menu_->clear();
00571 
00572   D_string::iterator it = recent_configs_.begin();
00573   D_string::iterator end = recent_configs_.end();
00574   for (; it != end; ++it)
00575   {
00576     if( *it != "" )
00577     {
00578       std::string display_name = *it;
00579       if( display_name == default_display_config_file_ )
00580       {
00581         display_name += " (default)";
00582       }
00583       if( display_name.find( home_dir_ ) == 0 )
00584       {
00585         display_name = ("~" / fs::path( display_name.substr( home_dir_.size() ))).BOOST_FILE_STRING();
00586       }
00587       QString qdisplay_name = QString::fromStdString( display_name );
00588       QAction* action = new QAction( qdisplay_name, this );
00589       action->setData( QString::fromStdString( *it ));
00590       connect( action, SIGNAL( triggered() ), this, SLOT( onRecentConfigSelected() ));
00591       recent_configs_menu_->addAction( action );
00592     }
00593   }
00594 }
00595 
00596 void VisualizationFrame::markRecentConfig( const std::string& path )
00597 {
00598   D_string::iterator it = std::find( recent_configs_.begin(), recent_configs_.end(), path );
00599   if( it != recent_configs_.end() )
00600   {
00601     recent_configs_.erase( it );
00602   }
00603 
00604   recent_configs_.push_front( path );
00605 
00606   if( recent_configs_.size() > RECENT_CONFIG_COUNT )
00607   {
00608     recent_configs_.pop_back();
00609   }
00610 
00611   updateRecentConfigMenu();
00612 }
00613 
00614 void VisualizationFrame::loadDisplayConfig( const QString& qpath )
00615 {
00616   std::string path = qpath.toStdString();
00617   std::string actual_load_path = path;
00618   if( !fs::exists( path ) || fs::is_directory( path ) || fs::is_empty( path ))
00619   {
00620     actual_load_path = (fs::path(package_path_) / "default.rviz").BOOST_FILE_STRING();      
00621     if( !fs::exists( actual_load_path ))
00622     {
00623       ROS_ERROR( "Default display config '%s' not found.  RViz will be very empty at first.", actual_load_path.c_str() );
00624       return;
00625     }
00626   }
00627 
00628   // Check if we have unsaved changes to the current config the same
00629   // as we do during exit, with the same option to cancel.
00630   if( !prepareToExit() )
00631   {
00632     return;
00633   }
00634 
00635   setWindowModified( false );
00636   loading_ = true;
00637 
00638   LoadingDialog* dialog = NULL;
00639   if( initialized_ )
00640   {
00641     dialog = new LoadingDialog( this );
00642     dialog->show();
00643     connect( this, SIGNAL( statusUpdate( const QString& )), dialog, SLOT( showMessage( const QString& )));
00644   }
00645 
00646   YamlConfigReader reader;
00647   Config config;
00648   reader.readFile( config, QString::fromStdString( actual_load_path ));
00649   if( !reader.error() )
00650   {
00651     load( config );
00652   }
00653 
00654   markRecentConfig( path );
00655 
00656   setDisplayConfigFile( path );
00657 
00658   last_config_dir_ = fs::path( path ).parent_path().BOOST_FILE_STRING();
00659 
00660   delete dialog;
00661 
00662   post_load_timer_->start( 1000 );
00663 }
00664 
00665 void VisualizationFrame::markLoadingDone()
00666 {
00667   loading_ = false;
00668 }
00669 
00670 void VisualizationFrame::setImageSaveDirectory( const QString& directory )
00671 {
00672   last_image_dir_ = directory.toStdString();
00673 }
00674 
00675 void VisualizationFrame::setDisplayConfigModified()
00676 {
00677   if( !loading_ )
00678   {
00679     setWindowModified( true );
00680   }
00681 }
00682 
00683 void VisualizationFrame::setDisplayConfigFile( const std::string& path )
00684 {
00685   display_config_file_ = path;
00686 
00687   std::string title;
00688   if( path == default_display_config_file_ )
00689   {
00690     title = "RViz[*]";
00691   }
00692   else
00693   {
00694     title = fs::path( path ).BOOST_FILENAME_STRING() + "[*] - RViz";
00695   }
00696   setWindowTitle( QString::fromStdString( title ));
00697 }
00698 
00699 bool VisualizationFrame::saveDisplayConfig( const QString& path )
00700 {
00701   Config config;
00702   save( config );
00703 
00704   YamlConfigWriter writer;
00705   writer.writeFile( config, path );
00706 
00707   if( writer.error() )
00708   {
00709     ROS_ERROR( "%s", qPrintable( writer.errorMessage() ));
00710     error_message_ = writer.errorMessage();
00711     return false;
00712   }
00713   else
00714   {
00715     setWindowModified( false );
00716     error_message_ = "";
00717     return true;
00718   }
00719 }
00720 
00721 void VisualizationFrame::save( Config config )
00722 {
00723   manager_->save( config.mapMakeChild( "Visualization Manager" ));
00724   savePanels( config.mapMakeChild( "Panels" ));
00725   saveWindowGeometry( config.mapMakeChild( "Window Geometry" ));
00726 }
00727 
00728 void VisualizationFrame::load( const Config& config )
00729 {
00730   manager_->load( config.mapGetChild( "Visualization Manager" ));
00731   loadPanels( config.mapGetChild( "Panels" ));
00732   loadWindowGeometry( config.mapGetChild( "Window Geometry" ));
00733 }
00734 
00735 void VisualizationFrame::loadWindowGeometry( const Config& config )
00736 {
00737   int x, y;
00738   if( config.mapGetInt( "X", &x ) &&
00739       config.mapGetInt( "Y", &y ))
00740   {
00741     move( x, y );
00742   }
00743 
00744   int width, height;
00745   if( config.mapGetInt( "Width", &width ) &&
00746       config.mapGetInt( "Height", &height ))
00747   {
00748     resize( width, height );
00749   }    
00750 
00751   QString main_window_config;
00752   if( config.mapGetString( "QMainWindow State", &main_window_config ))
00753   {
00754     restoreState( QByteArray::fromHex( qPrintable( main_window_config )));
00755   }
00756 
00757   // load panel dock widget states (collapsed or not)
00758   QList<PanelDockWidget *> dock_widgets = findChildren<PanelDockWidget *>();
00759 
00760   for ( QList<PanelDockWidget *>::iterator it=dock_widgets.begin(); it!=dock_widgets.end(); it++ )
00761   {
00762     Config itConfig = config.mapGetChild((*it)->windowTitle());
00763 
00764     if (itConfig.isValid())
00765     {
00766       (*it)->load(itConfig);
00767     }
00768   }
00769 
00770   bool b;
00771   config.mapGetBool( "Hide Left Dock", &b );
00772   hide_left_dock_button_->setChecked( b );
00773   hideLeftDock(b);
00774   config.mapGetBool( "Hide Right Dock", &b );
00775   hideRightDock(b);
00776   hide_right_dock_button_->setChecked( b );
00777 }
00778 
00779 void VisualizationFrame::saveWindowGeometry( Config config )
00780 {
00781   config.mapSetValue( "X", x() );
00782   config.mapSetValue( "Y", y() );
00783   config.mapSetValue( "Width", width() );
00784   config.mapSetValue( "Height", height() );
00785 
00786   QByteArray window_state = saveState().toHex();
00787   config.mapSetValue( "QMainWindow State", window_state.constData() );
00788 
00789   config.mapSetValue( "Hide Left Dock", hide_left_dock_button_->isChecked() );
00790   config.mapSetValue( "Hide Right Dock", hide_right_dock_button_->isChecked() );
00791 
00792   // save panel dock widget states (collapsed or not)
00793   QList<PanelDockWidget *> dock_widgets = findChildren<PanelDockWidget *>();
00794 
00795   for ( QList<PanelDockWidget *>::iterator it=dock_widgets.begin(); it!=dock_widgets.end(); it++ )
00796   {
00797     (*it)->save(config.mapMakeChild( (*it)->windowTitle() ));
00798   }
00799 }
00800 
00801 void VisualizationFrame::loadPanels( const Config& config )
00802 {
00803   // First destroy any existing custom panels.
00804   for( int i = 0; i < custom_panels_.size(); i++ )
00805   {
00806     delete custom_panels_[ i ].dock;
00807     delete custom_panels_[ i ].delete_action;
00808   }
00809   custom_panels_.clear();
00810 
00811   // Then load the ones in the config.
00812   int num_custom_panels = config.listLength();
00813   for( int i = 0; i < num_custom_panels; i++ )
00814   {
00815     Config panel_config = config.listChildAt( i );
00816 
00817     QString class_id, name;
00818     if( panel_config.mapGetString( "Class", &class_id ) &&
00819         panel_config.mapGetString( "Name", &name ))
00820     {
00821       QDockWidget* dock = addPanelByName( name, class_id );
00822       // This is kind of ridiculous - should just be something like
00823       // createPanel() and addPanel() so I can do load() without this
00824       // qobject_cast.
00825       if( dock )
00826       {
00827         Panel* panel = qobject_cast<Panel*>( dock->widget() );
00828         if( panel )
00829         {
00830           panel->load( panel_config );
00831         }
00832       }
00833     }
00834   }
00835 }
00836 
00837 void VisualizationFrame::savePanels( Config config )
00838 {
00839   config.setType( Config::List ); // Not really necessary, but gives an empty list if there are no entries, instead of an Empty config node.
00840 
00841   for( int i = 0; i < custom_panels_.size(); i++ )
00842   {
00843     custom_panels_[ i ].panel->save( config.listAppendNew() );
00844   }
00845 }
00846 
00847 bool VisualizationFrame::prepareToExit()
00848 {
00849   if( !initialized_ )
00850   {
00851     return true;
00852   }
00853 
00854   savePersistentSettings();
00855 
00856   if( isWindowModified() )
00857   {
00858     QMessageBox box( this );
00859     box.setText( "There are unsaved changes." );
00860     box.setInformativeText( QString::fromStdString( "Save changes to " + display_config_file_ + "?" ));
00861     box.setStandardButtons( QMessageBox::Save | QMessageBox::Discard | QMessageBox::Cancel );
00862     box.setDefaultButton( QMessageBox::Save );
00863     manager_->stopUpdate();
00864     int result = box.exec();
00865     manager_->startUpdate();
00866     switch( result )
00867     {
00868     case QMessageBox::Save:
00869       if( saveDisplayConfig( QString::fromStdString( display_config_file_ )))
00870       {
00871         return true;
00872       }
00873       else
00874       {
00875         QMessageBox box( this );
00876         box.setWindowTitle( "Failed to save." );
00877         box.setText( getErrorMessage() );
00878         box.setInformativeText( QString::fromStdString( "Save copy of " + display_config_file_ + " to another file?" ));
00879         box.setStandardButtons( QMessageBox::Save | QMessageBox::Discard | QMessageBox::Cancel );
00880         box.setDefaultButton( QMessageBox::Save );
00881         int result = box.exec();
00882         switch( result )
00883         {
00884         case QMessageBox::Save:
00885           onSaveAs();
00886           return true;
00887         case QMessageBox::Discard:
00888           return true;
00889         default:
00890           return false;
00891         }
00892         
00893       }
00894     case QMessageBox::Discard:
00895       return true;
00896     default:
00897       return false;
00898     }
00899   }
00900   else
00901   {
00902     return true;
00903   }
00904 }
00905 
00906 void VisualizationFrame::onOpen()
00907 {
00908   manager_->stopUpdate();
00909   QString filename = QFileDialog::getOpenFileName( this, "Choose a file to open",
00910                                                    QString::fromStdString( last_config_dir_ ),
00911                                                    "RViz config files (" CONFIG_EXTENSION_WILDCARD ")" );
00912   manager_->startUpdate();
00913 
00914   if( !filename.isEmpty() )
00915   {
00916     std::string path = filename.toStdString();
00917 
00918     if( !fs::exists( path ))
00919     {
00920       QString message = filename + " does not exist!";
00921       QMessageBox::critical( this, "Config file does not exist", message );
00922       return;
00923     }
00924 
00925     loadDisplayConfig( filename );
00926   }
00927 }
00928 
00929 void VisualizationFrame::onSave()
00930 {
00931   if( !initialized_ )
00932   {
00933     return;
00934   }
00935 
00936   savePersistentSettings();
00937 
00938   if( !saveDisplayConfig( QString::fromStdString( display_config_file_ )))
00939   {
00940     manager_->stopUpdate();
00941     QMessageBox box( this );
00942     box.setWindowTitle( "Failed to save." );
00943     box.setText( getErrorMessage() );
00944     box.setInformativeText( QString::fromStdString( "Save copy of " + display_config_file_ + " to another file?" ));
00945     box.setStandardButtons( QMessageBox::Save | QMessageBox::Cancel );
00946     box.setDefaultButton( QMessageBox::Save );
00947     if( box.exec() == QMessageBox::Save )
00948     {
00949       onSaveAs();
00950     }
00951     manager_->startUpdate();
00952   }
00953 }
00954 
00955 void VisualizationFrame::onSaveAs()
00956 {
00957   manager_->stopUpdate();
00958   QString q_filename = QFileDialog::getSaveFileName( this, "Choose a file to save to",
00959                                                      QString::fromStdString( last_config_dir_ ),
00960                                                      "RViz config files (" CONFIG_EXTENSION_WILDCARD ")" );
00961   manager_->startUpdate();
00962 
00963   if( !q_filename.isEmpty() )
00964   {
00965     std::string filename = q_filename.toStdString();
00966     fs::path path( filename );
00967     if( path.extension() != "."CONFIG_EXTENSION )
00968     {
00969       filename += "."CONFIG_EXTENSION;
00970     }
00971 
00972     if( !saveDisplayConfig( QString::fromStdString( filename )))
00973     {
00974       QMessageBox::critical( this, "Failed to save.", getErrorMessage() );
00975     }
00976 
00977     markRecentConfig( filename );
00978     last_config_dir_ = fs::path( filename ).parent_path().BOOST_FILE_STRING();
00979     setDisplayConfigFile( filename );
00980   }
00981 }
00982 
00983 void VisualizationFrame::onSaveImage()
00984 {
00985   ScreenshotDialog* dialog = new ScreenshotDialog( this, render_panel_, manager_, QString::fromStdString( last_image_dir_ ));
00986   connect( dialog, SIGNAL( savedInDirectory( const QString& )),
00987            this, SLOT( setImageSaveDirectory( const QString& )));
00988   dialog->show();
00989 }
00990 
00991 void VisualizationFrame::onRecentConfigSelected()
00992 {
00993   QAction* action = dynamic_cast<QAction*>( sender() );
00994   if( action )
00995   {
00996     std::string path = action->data().toString().toStdString();
00997     if( !path.empty() )
00998     {
00999       if( !fs::exists( path ))
01000       {
01001         QString message = QString::fromStdString( path  ) + " does not exist!";
01002         QMessageBox::critical( this, "Config file does not exist", message );
01003         return;
01004       }
01005 
01006       loadDisplayConfig( QString::fromStdString( path ));
01007     }
01008   }
01009 }
01010 
01011 void VisualizationFrame::addTool( Tool* tool )
01012 {
01013   QAction* action = new QAction( tool->getName(), toolbar_actions_ );
01014   action->setIcon( tool->getIcon() );
01015   action->setIconText( tool->getName() );
01016   action->setCheckable( true );
01017   action->setShortcut( QKeySequence( QString( tool->getShortcutKey() )));
01018   toolbar_->insertAction( add_tool_action_, action );
01019   action_to_tool_map_[ action ] = tool;
01020   tool_to_action_map_[ tool ] = action;
01021 
01022   remove_tool_menu_->addAction( tool->getName() );
01023 }
01024 
01025 void VisualizationFrame::onToolbarActionTriggered( QAction* action )
01026 {
01027   Tool* tool = action_to_tool_map_[ action ];
01028   if( tool )
01029   {
01030     manager_->getToolManager()->setCurrentTool( tool );
01031   }
01032 }
01033 
01034 void VisualizationFrame::onToolbarRemoveTool( QAction* remove_tool_menu_action )
01035 {
01036   QString name = remove_tool_menu_action->text();
01037   for( int i = 0; i < manager_->getToolManager()->numTools(); i++ )
01038   {
01039     Tool* tool = manager_->getToolManager()->getTool( i );
01040     if( tool->getName() == name )
01041     {
01042       manager_->getToolManager()->removeTool( i );
01043       return;
01044     }
01045   }
01046 }
01047 
01048 void VisualizationFrame::removeTool( Tool* tool )
01049 {
01050   QAction* action = tool_to_action_map_[ tool ];
01051   if( action )
01052   {
01053     toolbar_actions_->removeAction( action );
01054     toolbar_->removeAction( action );
01055     tool_to_action_map_.erase( tool );
01056     action_to_tool_map_.erase( action );
01057   }
01058   QString tool_name = tool->getName();
01059   QList<QAction*> remove_tool_actions = remove_tool_menu_->actions();
01060   for( int i = 0; i < remove_tool_actions.size(); i++ )
01061   {
01062     QAction* removal_action = remove_tool_actions.at( i );
01063     if( removal_action->text() == tool_name )
01064     {
01065       remove_tool_menu_->removeAction( removal_action );
01066       break;
01067     }
01068   }
01069 }
01070 
01071 void VisualizationFrame::indicateToolIsCurrent( Tool* tool )
01072 {
01073   QAction* action = tool_to_action_map_[ tool ];
01074   if( action )
01075   {
01076     action->setChecked( true );
01077   }
01078 }
01079 
01080 void VisualizationFrame::showHelpPanel()
01081 {
01082   if( !show_help_action_ )
01083   {
01084     QDockWidget* dock = addPanelByName( "Help", "rviz/Help" );
01085     show_help_action_ = dock->toggleViewAction();
01086     connect( dock, SIGNAL( destroyed( QObject* )), this, SLOT( onHelpDestroyed() ));
01087   }
01088   else
01089   {
01090     // show_help_action_ is a toggle action, so trigger() changes its
01091     // state.  Therefore we must force it to the opposite state from
01092     // what we want before we call trigger().  (I think.)
01093     show_help_action_->setChecked( false );
01094     show_help_action_->trigger();
01095   }
01096 }
01097 
01098 void VisualizationFrame::onHelpDestroyed()
01099 {
01100   show_help_action_ = NULL;
01101 }
01102 
01103 void VisualizationFrame::onHelpWiki()
01104 {
01105   QDesktopServices::openUrl( QUrl( "http://www.ros.org/wiki/rviz" ));
01106 }
01107 
01108 QWidget* VisualizationFrame::getParentWindow()
01109 {
01110   return this;
01111 }
01112 
01113 void VisualizationFrame::onDeletePanel()
01114 {
01115   // This should only be called as a SLOT from a QAction in the
01116   // "delete panel" submenu, so the sender will be one of the QActions
01117   // stored as "delete_action" in a PanelRecord.  This code looks for
01118   // a delete_action in custom_panels_ matching sender() and removes
01119   // the panel associated with it.
01120   if( QAction* action = qobject_cast<QAction*>( sender() ))
01121   {
01122     for( int i = 0; i < custom_panels_.size(); i++ )
01123     {
01124       if( custom_panels_[ i ].delete_action == action )
01125       {
01126         delete custom_panels_[ i ].dock;
01127         custom_panels_.removeAt( i );
01128         setDisplayConfigModified();
01129         action->deleteLater();
01130         if( delete_view_menu_->actions().size() == 1 &&
01131             delete_view_menu_->actions().first() == action )
01132         {
01133           delete_view_menu_->setEnabled( false );
01134         }
01135         return;
01136       }
01137     }
01138   }
01139 }
01140 
01141 QDockWidget* VisualizationFrame::addPanelByName( const QString& name,
01142                                                  const QString& class_id,
01143                                                  Qt::DockWidgetArea area,
01144                                                  bool floating )
01145 {
01146   QString error;
01147   Panel* panel = panel_factory_->make( class_id, &error );
01148   if( !panel )
01149   {
01150     panel = new FailedPanel( class_id, error );
01151   }
01152   panel->setName( name );
01153   connect( panel, SIGNAL( configChanged() ), this, SLOT( setDisplayConfigModified() ));
01154 
01155   PanelRecord record;
01156   record.dock = addPane( name, panel, area, floating );
01157   record.panel = panel;
01158   record.name = name;
01159   record.delete_action = delete_view_menu_->addAction( name, this, SLOT( onDeletePanel() ));
01160   custom_panels_.append( record );
01161   delete_view_menu_->setEnabled( true );
01162 
01163   record.panel->initialize( manager_ );
01164 
01165   record.dock->setIcon( panel_factory_->getIcon( class_id ) );
01166 
01167   return record.dock;
01168 }
01169 
01170 PanelDockWidget* VisualizationFrame::addPane( const QString& name, QWidget* panel, Qt::DockWidgetArea area, bool floating )
01171 {
01172   PanelDockWidget *dock;
01173   dock = new PanelDockWidget( name );
01174   dock->setContentWidget( panel );
01175   dock->setFloating( floating );
01176   dock->setObjectName( name ); // QMainWindow::saveState() needs objectName to be set.
01177   addDockWidget( area, dock );
01178 
01179   // we want to know when that panel becomes visible
01180   connect( dock, SIGNAL( visibilityChanged( bool )), this, SLOT( onDockPanelVisibilityChange( bool ) ));
01181 
01182   QAction* toggle_action = dock->toggleViewAction();
01183   view_menu_->addAction( toggle_action );
01184 
01185   connect( toggle_action, SIGNAL( triggered( bool )), this, SLOT( setDisplayConfigModified() ));
01186   connect( dock, SIGNAL( closed()), this, SLOT( setDisplayConfigModified() ));
01187 
01188   dock->installEventFilter( geom_change_detector_ );
01189 
01190   // repair/update visibility status
01191   hideLeftDock( area == Qt::LeftDockWidgetArea ? false : hide_left_dock_button_->isChecked() );
01192   hideRightDock( area == Qt::RightDockWidgetArea ? false : hide_right_dock_button_->isChecked() );
01193 
01194   return dock;
01195 }
01196 
01197 } // end namespace rviz


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