00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 import sys
00019 
00020 
00021 import os
00022 import rospy
00023 from rospkg.rospack import RosPack
00024 
00025 from rqt_gui.main import Main
00026 from rqt_gui.main import Base
00027 
00028 from argparse import ArgumentParser, SUPPRESS
00029 import platform
00030 import signal
00031 import sys
00032 
00033 from python_qt_binding.QtGui import *
00034 from python_qt_binding.QtCore import *
00035 
00036 from airbus_cobot_gui import Plugin
00037 
00038 class RqtGuiWrapperClass(Main):
00039     
00040     def __init__(self, filename=None, ros_pack=None):
00041         rp = ros_pack or RosPack()
00042         qtgui_path = rp.get_path('qt_gui')
00043         super(Main, self).__init__(qtgui_path, invoked_filename=filename, settings_filename='rqt_gui')
00044         self._ros_pack = rp
00045     
00046     def main(self, argv=None, standalone=None, plugin_argument_provider=None):
00047         if argv is None:
00048             argv = sys.argv
00049         
00050         
00051         
00052         arguments = []
00053         
00054         if not standalone:
00055             plugin_args = []
00056             if '--args' in arguments:
00057                 index = arguments.index('--args')
00058                 plugin_args = arguments[index + 1:]
00059                 arguments = arguments[0:index + 1]
00060         
00061         parser = ArgumentParser(os.path.basename(Main.main_filename),
00062                                 add_help=False)
00063         self.add_arguments(parser, standalone=bool(standalone),
00064                            plugin_argument_provider=plugin_argument_provider)
00065         self._options = parser.parse_args(arguments)
00066         
00067         if standalone:
00068             
00069             parser = ArgumentParser(os.path.basename(Main.main_filename),
00070                                     add_help=False)
00071             self.add_arguments(parser, standalone=bool(standalone))
00072             self._options, plugin_args = parser.parse_known_args(arguments)
00073         self._options.plugin_args = plugin_args
00074         
00075         
00076         if standalone:
00077             self._options.freeze_layout = False
00078             self._options.lock_perspective = False
00079             self._options.multi_process = False
00080             self._options.perspective = None
00081             self._options.perspective_file = None
00082             self._options.standalone_plugin = standalone
00083             self._options.list_perspectives = False
00084             self._options.list_plugins = False
00085             self._options.command_pid = None
00086             self._options.command_start_plugin = None
00087             self._options.command_switch_perspective = None
00088             self._options.embed_plugin = None
00089             self._options.embed_plugin_serial = None
00090             self._options.embed_plugin_address = None
00091         
00092         
00093         try:
00094             if self._options.plugin_args and \
00095             not self._options.standalone_plugin and \
00096             not self._options.command_start_plugin and \
00097             not self._options.embed_plugin:
00098                 raise RuntimeError('Option --args can only be used together with either --standalone, --command-start-plugin or --embed-plugin option')
00099             
00100             if self._options.freeze_layout and \
00101                not self._options.lock_perspective:
00102                 raise RuntimeError('Option --freeze_layout can only be used together with the --lock_perspective option')
00103             
00104             list_options = (self._options.list_perspectives,
00105                             self._options.list_plugins)
00106             list_options_set = [opt for opt in list_options if opt is not False]
00107             if len(list_options_set) > 1:
00108                 raise RuntimeError('Only one --list-* option can be used at a time')
00109             
00110             command_options = (self._options.command_start_plugin,
00111                                self._options.command_switch_perspective)
00112             command_options_set = [opt for opt in command_options if opt is not None]
00113             if len(command_options_set) > 0 and not self._dbus_available:
00114                 raise RuntimeError('Without DBus support the --command-* options are not available')
00115             if len(command_options_set) > 1:
00116                 raise RuntimeError('Only one --command-* option can be used at a time (except --command-pid which is optional)')
00117             if len(command_options_set) == 0 and \
00118                self._options.command_pid is not None:
00119                 raise RuntimeError('Option --command_pid can only be used together with an other --command-* option')
00120             
00121             embed_options = (self._options.embed_plugin,
00122                              self._options.embed_plugin_serial,
00123                              self._options.embed_plugin_address)
00124             embed_options_set = [opt for opt in embed_options if opt is not None]
00125             if len(command_options_set) > 0 and not self._dbus_available:
00126                 raise RuntimeError('Without DBus support the --embed-* options are not available')
00127             if len(embed_options_set) > 0 and \
00128                len(embed_options_set) < len(embed_options):
00129                 raise RuntimeError('Missing option(s) - all \'--embed-*\' options must be set')
00130             
00131             if len(embed_options_set) > 0 and self._options.clear_config:
00132                 raise RuntimeError('Option --clear-config can only be used without any --embed-* option')
00133             
00134             groups = (list_options_set, command_options_set, embed_options_set)
00135             groups_set = [opt for opt in groups if len(opt) > 0]
00136             if len(groups_set) > 1:
00137                 raise RuntimeError('Options from different groups (--list, --command, --embed) can not be used together')
00138             
00139             perspective_options = (self._options.perspective,
00140                                    self._options.perspective_file)
00141             perspective_options_set = [opt for opt in perspective_options if opt is not None]
00142             if len(perspective_options_set) > 1:
00143                 raise RuntimeError('Only one --perspective-* option can be used at a time')
00144             
00145             if self._options.perspective_file is not None and \
00146                not os.path.isfile(self._options.perspective_file):
00147                 raise RuntimeError('Option --perspective-file must reference existing file')
00148         
00149         except RuntimeError as e:
00150             print(str(e))
00151             
00152             
00153             return 1
00154         
00155         
00156         if self._options.standalone_plugin is not None:
00157             self._options.lock_perspective = True
00158         
00159         
00160         from qt_gui.application_context import ApplicationContext
00161         context = ApplicationContext()
00162         context.qtgui_path = self._qtgui_path
00163         context.options = self._options
00164         
00165         if self._dbus_available:
00166             from dbus import DBusException, Interface, SessionBus
00167         
00168         
00169         if self._dbus_available:
00170             context.provide_app_dbus_interfaces = len(groups_set) == 0
00171             context.dbus_base_bus_name = 'org.ros.qt_gui'
00172             if context.provide_app_dbus_interfaces:
00173                 context.dbus_unique_bus_name = context.dbus_base_bus_name + '.pid%d' % os.getpid()
00174                 
00175                 
00176                 from qt_gui.application_dbus_interface import ApplicationDBusInterface
00177                 _dbus_server = ApplicationDBusInterface(context.dbus_base_bus_name)
00178         
00179         
00180         if len(command_options_set) > 0 or len(embed_options_set) > 0:
00181             host_pid = None
00182             if self._options.command_pid is not None:
00183                 host_pid = self._options.command_pid
00184             else:
00185                 try:
00186                     remote_object = SessionBus().get_object(context.dbus_base_bus_name,
00187                                                             '/Application')
00188                 except DBusException:
00189                     pass
00190                 else:
00191                     remote_interface = Interface(remote_object,
00192                                                  context.dbus_base_bus_name + '.Application')
00193                     host_pid = remote_interface.get_pid()
00194             if host_pid is not None:
00195                 context.dbus_host_bus_name = context.dbus_base_bus_name + '.pid%d' % host_pid
00196         
00197         
00198         if len(command_options_set) > 0:
00199             if self._options.command_start_plugin is not None:
00200                 try:
00201                     remote_object = SessionBus().get_object(context.dbus_host_bus_name,
00202                                                             '/PluginManager')
00203                 except DBusException:
00204                     (rc, msg) = (1, 
00205                                  'unable to communicate with GUI instance "%s"'
00206                                  % context.dbus_host_bus_name)
00207                 else:
00208                     remote_interface = Interface(remote_object,
00209                                                  context.dbus_base_bus_name + '.PluginManager')
00210                     (rc, msg) = remote_interface.start_plugin(self._options.command_start_plugin,
00211                                                               ' '.join(self._options.plugin_args))
00212                 if rc == 0:
00213                     print('qt_gui_main() started plugin "%s" in GUI "%s"'
00214                           %(msg, context.dbus_host_bus_name))
00215                 else:
00216                     print('qt_gui_main() could not start plugin "%s" in GUI "%s": %s'
00217                           %(self._options.command_start_plugin,
00218                             context.dbus_host_bus_name, msg))
00219                 return rc
00220             elif self._options.command_switch_perspective is not None:
00221                 remote_object = SessionBus().get_object(context.dbus_host_bus_name,
00222                                                         '/PerspectiveManager')
00223                 remote_interface = Interface(remote_object,
00224                                              context.dbus_base_bus_name+'.PerspectiveManager')
00225                 remote_interface.switch_perspective(self._options.command_switch_perspective)
00226                 print('qt_gui_main() switched to perspective "%s" in GUI "%s"'
00227                       %(self._options.command_switch_perspective,
00228                         context.dbus_host_bus_name))
00229                 return 0
00230             raise RuntimeError('Unknown command not handled')
00231         
00232         
00233         setattr(sys, 'SELECT_QT_BINDING', self._options.qt_binding)
00234         from python_qt_binding import QT_BINDING
00235         
00236         from python_qt_binding.QtCore import qDebug, qInstallMsgHandler, \
00237                                              QSettings, Qt, QtCriticalMsg, \
00238                                              QtDebugMsg, QtFatalMsg, \
00239                                              QTimer, QtWarningMsg
00240         from python_qt_binding.QtGui import QAction, QIcon, QMenuBar
00241         
00242         from qt_gui.about_handler import AboutHandler
00243         from qt_gui.composite_plugin_provider import CompositePluginProvider
00244         from qt_gui.container_manager import ContainerManager
00245         from qt_gui.help_provider import HelpProvider
00246         from qt_gui.main_window import MainWindow
00247         from qt_gui.minimized_dock_widgets_toolbar import MinimizedDockWidgetsToolbar
00248         from qt_gui.perspective_manager import PerspectiveManager
00249         from qt_gui.plugin_manager import PluginManager
00250         
00251         def message_handler(type_, msg):
00252             colored_output = 'TERM' in os.environ and 'ANSI_COLORS_DISABLED' not in os.environ
00253             cyan_color = '\033[36m' if colored_output else ''
00254             red_color = '\033[31m' if colored_output else ''
00255             reset_color = '\033[0m' if colored_output else ''
00256             if type_ == QtDebugMsg and self._options.verbose:
00257                 print(msg, sys.stderr)
00258             elif type_ == QtWarningMsg:
00259                 print(cyan_color + msg + reset_color, sys.stderr)
00260             elif type_ == QtCriticalMsg:
00261                 print(red_color + msg + reset_color, sys.stderr)
00262             elif type_ == QtFatalMsg:
00263                 print(red_color + msg + reset_color, sys.stderr)
00264                 sys.exit(1)
00265                 
00266         qInstallMsgHandler(message_handler)
00267         
00268         app = self.create_application(argv)
00269         
00270         self._check_icon_theme_compliance()
00271         
00272         settings = QSettings(QSettings.IniFormat,
00273                              QSettings.UserScope,
00274                              'ros.org',
00275                              self._settings_filename)
00276         if len(embed_options_set) == 0:
00277             if self._options.clear_config:
00278                 settings.clear()
00279             
00280             main_window = MainWindow()
00281             
00282             if self._options.on_top:
00283                 main_window.setWindowFlags(Qt.WindowStaysOnTopHint)
00284             
00285             main_window.statusBar()
00286             
00287             def sigint_handler(*args):
00288                 qDebug('\nsigint_handler()')
00289                 main_window.close()
00290             signal.signal(signal.SIGINT, sigint_handler)
00291             
00292             timer = QTimer()
00293             timer.start(500)
00294             timer.timeout.connect(lambda: None)
00295             
00296             
00297             menu_bar = QMenuBar()
00298             if 'darwin' in platform.platform().lower():
00299                 menu_bar.setNativeMenuBar(True)
00300             else:
00301                 menu_bar.setNativeMenuBar(False)
00302             if not self._options.lock_perspective:
00303                 main_window.setMenuBar(menu_bar)
00304             
00305             file_menu = menu_bar.addMenu(menu_bar.tr('File'))
00306             action = QAction(file_menu.tr('Quit'), file_menu)
00307             action.setIcon(QIcon.fromTheme('application-exit'))
00308             action.triggered.connect(main_window.close)
00309             file_menu.addAction(action)
00310             
00311         else:
00312             app.setQuitOnLastWindowClosed(False)
00313             
00314             main_window = None
00315             menu_bar = None
00316         
00317         self._add_plugin_providers()
00318         
00319         
00320         plugin_provider = CompositePluginProvider(self.plugin_providers)
00321         plugin_manager = PluginManager(plugin_provider, settings, context)
00322         
00323         if self._options.list_plugins:
00324             
00325             print('\n'.join(sorted(plugin_manager.get_plugins().values())))
00326             return 0
00327         
00328         help_provider = HelpProvider()
00329         plugin_manager.plugin_help_signal.connect(help_provider.plugin_help_request)
00330         
00331         
00332         if main_window is not None:
00333             perspective_manager = PerspectiveManager(settings, context)
00334         
00335             if self._options.list_perspectives:
00336                 
00337                 print('\n'.join(sorted(perspective_manager.perspectives)))
00338                 return 0
00339         else:
00340             perspective_manager = None
00341             
00342         if main_window is not None:
00343             container_manager = ContainerManager(main_window, plugin_manager)
00344             plugin_manager.set_main_window(main_window, menu_bar, container_manager)
00345 
00346             if not self._options.freeze_layout:
00347                 minimized_dock_widgets_toolbar = MinimizedDockWidgetsToolbar(container_manager, main_window)
00348                 main_window.addToolBar(Qt.BottomToolBarArea, minimized_dock_widgets_toolbar)
00349                 plugin_manager.set_minimized_dock_widgets_toolbar(minimized_dock_widgets_toolbar)
00350 
00351         if menu_bar is not None:
00352             perspective_menu = menu_bar.addMenu(menu_bar.tr('P&erspectives'))
00353             perspective_manager.set_menu(perspective_menu)
00354             
00355         
00356         
00357         if perspective_manager is not None and main_window is not None:
00358             
00359             perspective_manager.perspective_changed_signal.connect(main_window.perspective_changed)
00360             
00361             perspective_manager.save_settings_signal.connect(main_window.save_settings)
00362             perspective_manager.restore_settings_signal.connect(main_window.restore_settings)
00363             perspective_manager.restore_settings_without_plugin_changes_signal.connect(main_window.restore_settings)
00364         
00365         if perspective_manager is not None and plugin_manager is not None:
00366             perspective_manager.save_settings_signal.connect(plugin_manager.save_settings)
00367             plugin_manager.save_settings_completed_signal.connect(perspective_manager.save_settings_completed)
00368             perspective_manager.restore_settings_signal.connect(plugin_manager.restore_settings)
00369             perspective_manager.restore_settings_without_plugin_changes_signal.connect(plugin_manager.restore_settings_without_plugins)
00370         
00371         if plugin_manager is not None and main_window is not None:
00372             
00373             plugin_manager.plugins_about_to_change_signal.connect(main_window.save_setup)
00374             
00375             plugin_manager.plugins_changed_signal.connect(main_window.restore_state)
00376             
00377             main_window.save_settings_before_close_signal.connect(plugin_manager.close_application)
00378             
00379             plugin_manager.close_application_signal.connect(main_window.close,
00380                                                             type=Qt.QueuedConnection)
00381         
00382         if main_window is not None and menu_bar is not None:
00383             about_handler = AboutHandler(context.qtgui_path, main_window)
00384             help_menu = menu_bar.addMenu(menu_bar.tr('Help'))
00385             action = QAction(file_menu.tr('About'), help_menu)
00386             action.setIcon(QIcon.fromTheme('help-about'))
00387             action.triggered.connect(about_handler.show)
00388             help_menu.addAction(action)
00389         
00390         
00391         if main_window is not None:
00392             main_window.resize(600, 450)
00393             main_window.move(100, 100)
00394         
00395         
00396         src_path = os.path.realpath(os.path.join(os.path.dirname(__file__), '..'))
00397         if src_path not in sys.path:
00398             sys.path.append(src_path)
00399         
00400         
00401         plugin = None
00402         plugin_serial = None
00403         if self._options.embed_plugin is not None:
00404             plugin = self._options.embed_plugin
00405             plugin_serial = self._options.embed_plugin_serial
00406         elif self._options.standalone_plugin is not None:
00407             plugin = self._options.standalone_plugin
00408             plugin_serial = 0
00409         if plugin is not None:
00410             plugins = plugin_manager.find_plugins_by_name(plugin)
00411             if len(plugins) == 0:
00412                 print('qt_gui_main() found no plugin matching "%s"' % plugin)
00413                 return 1
00414             elif len(plugins) > 1:
00415                 print('qt_gui_main() found multiple plugins matching "%s"\n%s'
00416                       %(plugin, '\n'.join(plugins.values())))
00417                 return 1
00418             plugin = plugins.keys()[0]
00419         
00420         qDebug('QtBindingHelper using %s' % QT_BINDING)
00421         
00422         plugin_manager.discover()
00423         
00424         if self._options.reload_import:
00425             qDebug('ReloadImporter() automatically reload all subsequent imports')
00426             from .reload_importer import ReloadImporter
00427             _reload_importer = ReloadImporter()
00428             self._add_reload_paths(_reload_importer)
00429             _reload_importer.enable()
00430         
00431         
00432         if perspective_manager is not None:
00433             if plugin:
00434                 perspective_manager.set_perspective(plugin,
00435                                                     hide_and_without_plugin_changes=True)
00436             elif self._options.perspective_file:
00437                 perspective_manager.import_perspective_from_file(self._options.perspective_file,
00438                                                                  perspective_manager.HIDDEN_PREFIX + '__cli_perspective_from_file')
00439             else:
00440                 perspective_manager.set_perspective(self._options.perspective)
00441         
00442         
00443         if plugin:
00444             plugin_manager.load_plugin(plugin, plugin_serial,
00445                                        self._options.plugin_args)
00446             running = plugin_manager.is_plugin_running(plugin, plugin_serial)
00447             if not running:
00448                 return 1
00449         
00450         return main_window
00451     
00452     def create_application(self, argv):
00453         pass
00454     
00455     def _add_plugin_providers(self):
00456         
00457         from qt_gui.recursive_plugin_provider import RecursivePluginProvider
00458         from rqt_gui.rospkg_plugin_provider import RospkgPluginProvider
00459         RospkgPluginProvider.rospack = self._ros_pack
00460         self.plugin_providers.append(RospkgPluginProvider('qt_gui',
00461                                                           'qt_gui_py::Plugin'))
00462         self.plugin_providers.append(RecursivePluginProvider(RospkgPluginProvider('qt_gui',
00463                                                                                   'qt_gui_py::PluginProvider')))
00464         self.plugin_providers.append(RecursivePluginProvider(RospkgPluginProvider('rqt_gui',
00465                                                                                   'rqt_gui_py::PluginProvider')))
00466     
00467     def _add_reload_paths(self, reload_importer):
00468         super(Main, self)._add_reload_paths(reload_importer)
00469         reload_importer.add_reload_path(os.path.join(os.path.dirname(__file__),
00470                                                      *('..',) * 4))
00471 
00472 class PluginRqt(Plugin):
00473      
00474     def __init__(self, context):
00475         Plugin.__init__(self, context)
00476         
00477         self._wrapper_rqt = None
00478         
00479     def onCreate(self, param):
00480         
00481         layout = QGridLayout(self)
00482         layout.setContentsMargins(0, 0, 0, 0)
00483         
00484         self._wrapper_rqt = RqtGuiWrapperClass()
00485         
00486         layout.addWidget(self._wrapper_rqt.main(), 0, 0, 0, 0)
00487         
00488     def onPause(self):
00489         pass
00490     
00491     def onResume(self):
00492         pass
00493     
00494     def onControlModeChanged(self, mode):
00495         pass
00496         
00497     def onUserChanged(self, user_info):
00498         pass
00499     
00500     def onTranslate(self, lng):
00501         pass
00502     
00503     def onEmergencyStop(self, state):
00504         pass
00505     
00506     def onDestroy(self):
00507         pass
00508     
00509     
00510