00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00044
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
00190
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
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
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 }