visualizer_app.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 <QApplication>
00031 #include <QTimer>
00032 
00033 #include <boost/thread.hpp>
00034 #include <boost/program_options.hpp>
00035 #include <signal.h>
00036 
00037 #include <OGRE/OgreHighLevelGpuProgramManager.h>
00038 #include <OGRE/OgreLogManager.h>
00039 #include <std_srvs/Empty.h>
00040 
00041 #ifdef Q_OS_MAC
00042 #include <ApplicationServices/ApplicationServices.h>
00043 // Apparently OSX #defines 'check' to be an empty string somewhere.  
00044 // That was fun to figure out.
00045 #undef check
00046 #endif
00047 
00048 #include <ros/ros.h>
00049 
00050 #include "visualization_frame.h"
00051 #include "version.h"
00052 #include "wait_for_master_dialog.h"
00053 #include "visualizer_app.h"
00054 
00055 #define CATCH_EXCEPTIONS 0
00056 
00057 namespace po = boost::program_options;
00058 
00059 namespace rviz
00060 {
00061 
00062 bool reloadShaders(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00063 {
00064   Ogre::ResourceManager::ResourceMapIterator it = Ogre::HighLevelGpuProgramManager::getSingleton().getResourceIterator();
00065   while (it.hasMoreElements())
00066   {
00067     Ogre::ResourcePtr resource = it.getNext();
00068     resource->reload();
00069   }
00070   return true;
00071 }
00072 
00073 VisualizerApp::VisualizerApp()
00074   : timer_( 0 )
00075   , frame_( 0 )
00076 {
00077 }
00078 
00079 void VisualizerApp::onTimer()
00080 {
00081   if( !continue_ )
00082   {
00083     QApplication::closeAllWindows();
00084   }
00085 }
00086 
00087 bool VisualizerApp::init( int argc, char** argv )
00088 {
00089   ROS_INFO( "rviz revision number %s", get_version().c_str() );
00090   ROS_INFO( "compiled against OGRE version %d.%d.%d%s (%s)",
00091             OGRE_VERSION_MAJOR, OGRE_VERSION_MINOR, OGRE_VERSION_PATCH,
00092             OGRE_VERSION_SUFFIX, OGRE_VERSION_NAME );
00093 
00094 #ifdef Q_OS_MAC
00095   ProcessSerialNumber PSN;
00096   GetCurrentProcess(&PSN);
00097   TransformProcessType(&PSN,kProcessTransformToForegroundApplication);
00098   SetFrontProcess(&PSN);
00099 #endif
00100 
00101 #if CATCH_EXCEPTIONS
00102   try
00103   {
00104 #endif
00105     ros::init( argc, argv, "rviz", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler );
00106 
00107     po::options_description options;
00108     options.add_options()
00109       ("help,h", "Produce this help message")
00110       ("splash-screen,s", po::value<std::string>(), "A custom splash-screen image to display")
00111       ("help-file", po::value<std::string>(), "A custom html file to show as the help screen")
00112       ("display-config,d", po::value<std::string>(), "A display config file (.vcg) to load")
00113       ("target-frame,t", po::value<std::string>(), "Set the target frame")
00114       ("fixed-frame,f", po::value<std::string>(), "Set the fixed frame")
00115       ("ogre-log,l", "Enable the Ogre.log file (output in cwd)")
00116       ("in-mc-wrapper", "Signal that this is running inside a master-chooser wrapper")
00117       ("verbose,v", "Enable debug visualizations");
00118     po::variables_map vm;
00119     std::string display_config, target_frame, fixed_frame, splash_path, help_path;
00120     bool enable_ogre_log = false;
00121     bool in_mc_wrapper = false;
00122     bool verbose = false;
00123     try
00124     {
00125       po::store( po::parse_command_line( argc, argv, options ), vm );
00126       po::notify( vm );
00127 
00128       if( vm.count( "help" ))
00129       {
00130         std::cout << "rviz command line options:\n" << options;
00131         return false;
00132       }
00133 
00134       if( vm.count( "in-mc-wrapper" ))
00135       {
00136         in_mc_wrapper = true;
00137       }
00138 
00139       if (vm.count("display-config"))
00140       {
00141         display_config = vm["display-config"].as<std::string>();
00142       }
00143 
00144       if (vm.count("splash-screen"))
00145       {
00146         splash_path = vm["splash-screen"].as<std::string>();
00147       }
00148 
00149       if (vm.count("help-file"))
00150       {
00151         help_path = vm["help-file"].as<std::string>();
00152       }
00153 
00154       if (vm.count("target-frame"))
00155       {
00156         target_frame = vm["target-frame"].as<std::string>();
00157       }
00158 
00159       if (vm.count("fixed-frame"))
00160       {
00161         fixed_frame = vm["fixed-frame"].as<std::string>();
00162       }
00163 
00164       if (vm.count("ogre-log"))
00165       {
00166         enable_ogre_log = true;
00167       }
00168 
00169       if (vm.count("verbose"))
00170       {
00171         verbose = true;
00172       }
00173     }
00174     catch (std::exception& e)
00175     {
00176       ROS_ERROR("Error parsing command line: %s", e.what());
00177       return false;
00178     }
00179 
00180     if( !ros::master::check() )
00181     {
00182       WaitForMasterDialog* dialog = new WaitForMasterDialog;
00183       if( dialog->exec() != QDialog::Accepted )
00184       {
00185         return false;
00186       }
00187     }
00188 
00189     // block kill signals on all threads, since this also disables signals in threads
00190     // created by this one (the main thread)
00191     sigset_t sig_set;
00192     sigemptyset(&sig_set);
00193     sigaddset(&sig_set, SIGKILL);
00194     sigaddset(&sig_set, SIGTERM);
00195     sigaddset(&sig_set, SIGQUIT);
00196     sigaddset(&sig_set, SIGINT);
00197     pthread_sigmask(SIG_BLOCK, &sig_set, NULL);
00198 
00199     // Start up our signal handler
00200     continue_ = true;
00201     signal_handler_thread_ = boost::thread(boost::bind(&VisualizerApp::signalHandler, this));
00202 
00203     nh_.reset( new ros::NodeHandle );
00204 
00205     Ogre::LogManager* log_manager = new Ogre::LogManager();
00206     log_manager->createLog( "Ogre.log", false, false, !enable_ogre_log );
00207 
00208     frame_ = new VisualizationFrame;
00209     frame_->initialize( display_config, fixed_frame, target_frame,
00210                         splash_path, help_path, verbose, in_mc_wrapper );
00211     frame_->show();
00212 
00213     timer_ = new QTimer( this );
00214     connect( timer_, SIGNAL( timeout() ), this, SLOT( onTimer() ));
00215     timer_->start( 100 );
00216 
00217     ros::NodeHandle private_nh("~");
00218     reload_shaders_service_ = private_nh.advertiseService("reload_shaders", reloadShaders);
00219 #if CATCH_EXCEPTIONS
00220   }
00221   catch (std::exception& e)
00222   {
00223     ROS_ERROR("Caught exception while loading: %s", e.what());
00224     return false;
00225   }
00226 #endif
00227   return true;
00228 }
00229 
00230 VisualizerApp::~VisualizerApp()
00231 {
00232   if( timer_ )
00233   {
00234     timer_->stop();
00235   }
00236   continue_ = false;
00237 
00238   delete frame_;
00239 
00240   raise(SIGQUIT);
00241 
00242   signal_handler_thread_.join();
00243 }
00244 
00245 void VisualizerApp::signalHandler()
00246 {
00247   sigset_t signal_set;
00248   while(continue_)
00249   {
00250     // Wait for any signals
00251     sigfillset(&signal_set);
00252 
00253 #if defined(Q_OS_MAC)
00254     int sig;
00255     sigwait(&signal_set, &sig);
00256 #else
00257     struct timespec ts = {0, 100000000};
00258     int sig = sigtimedwait(&signal_set, NULL, &ts);
00259 #endif
00260 
00261     switch( sig )
00262     {
00263     case SIGKILL:
00264     case SIGTERM:
00265     case SIGQUIT:
00266     {
00267       exit(1);
00268     }
00269     break;
00270 
00271     case SIGINT:
00272     {
00273       ros::shutdown();
00274       continue_ = false;
00275       return;
00276     }
00277     break;
00278 
00279     default:
00280       break;
00281     }
00282   }
00283 }
00284 
00285 } // namespace rviz


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:33