ros_py_plugin_provider.py
Go to the documentation of this file.
00001 # Copyright (c) 2011, Dirk Thomas, TU Darmstadt
00002 # All rights reserved.
00003 #
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions
00006 # 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
00011 #     copyright notice, this list of conditions and the following
00012 #     disclaimer in the documentation and/or other materials provided
00013 #     with the distribution.
00014 #   * Neither the name of the TU Darmstadt nor the names of its
00015 #     contributors may be used to endorse or promote products derived
00016 #     from this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00019 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00020 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00021 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00022 # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00023 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00024 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00026 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00027 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00028 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 # POSSIBILITY OF SUCH DAMAGE.
00030 
00031 import os
00032 import threading
00033 import time
00034 
00035 import rospy
00036 
00037 from qt_gui.composite_plugin_provider import CompositePluginProvider
00038 from qt_gui.errors import PluginLoadError
00039 
00040 from python_qt_binding.QtCore import qDebug, Qt, qWarning, Signal
00041 from python_qt_binding.QtGui import QMessageBox
00042 
00043 from rqt_gui.rospkg_plugin_provider import RospkgPluginProvider
00044 
00045 
00046 class RosPyPluginProvider(CompositePluginProvider):
00047 
00048     _master_found_signal = Signal(int)
00049 
00050     def __init__(self):
00051         super(RosPyPluginProvider, self).__init__([RospkgPluginProvider('rqt_gui', 'rqt_gui_py::Plugin')])
00052         self.setObjectName('RosPyPluginProvider')
00053         self._node_initialized = False
00054         self._wait_for_master_dialog = None
00055         self._wait_for_master_thread = None
00056 
00057     def load(self, plugin_id, plugin_context):
00058         self._check_for_master()
00059         self._init_node()
00060         return super(RosPyPluginProvider, self).load(plugin_id, plugin_context)
00061 
00062     def _check_for_master(self):
00063         # check if master is available
00064         try:
00065             rospy.get_master().getSystemState()
00066             return
00067         except Exception:
00068             pass
00069         # spawn thread to detect when master becomes available
00070         self._wait_for_master_thread = threading.Thread(target=self._wait_for_master)
00071         self._wait_for_master_thread.start()
00072         self._wait_for_master_dialog = QMessageBox(QMessageBox.Question, self.tr('Waiting for ROS master'), self.tr("Could not find ROS master. Either start a 'roscore' or abort loading the plugin."), QMessageBox.Abort)
00073         self._master_found_signal.connect(self._wait_for_master_dialog.done, Qt.QueuedConnection)
00074         button = self._wait_for_master_dialog.exec_()
00075         # check if master existence was not detected by background thread
00076         no_master = button != QMessageBox.Ok
00077         self._wait_for_master_dialog.deleteLater()
00078         self._wait_for_master_dialog = None
00079         if no_master:
00080             raise PluginLoadError('RosPyPluginProvider._init_node() could not find ROS master')
00081 
00082     def _wait_for_master(self):
00083         while True:
00084             time.sleep(0.1)
00085             if not self._wait_for_master_dialog:
00086                 break
00087             try:
00088                 rospy.get_master().getSystemState()
00089             except Exception:
00090                 continue
00091             self._master_found_signal.emit(QMessageBox.Ok)
00092             break
00093 
00094     def _init_node(self):
00095         # initialize node once
00096         if not self._node_initialized:
00097             name = 'rqt_gui_py_node_%d' % os.getpid()
00098             qDebug('RosPyPluginProvider._init_node() initialize ROS node "%s"' % name)
00099             rospy.init_node(name, disable_signals=True)
00100             self._node_initialized = True


rqt_gui_py
Author(s): Dirk Thomas
autogenerated on Thu Aug 27 2015 14:56:53