moveit_widget.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2012, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 # Author: Isaac Saito
00034 
00035 import os
00036 import sys
00037 from threading import Thread
00038 import time
00039 
00040 from python_qt_binding import loadUi
00041 from python_qt_binding.QtCore import QModelIndex, QTimer, Signal
00042 from python_qt_binding.QtGui import QStandardItem, QStandardItemModel, QWidget
00043 import rospkg
00044 #from rosnode import rosnode_ping, ROSNodeIOException
00045 #from rosnode import ROSNodeIOException
00046 import rospy
00047 import rostopic
00048 from rqt_py_common.rqt_roscomm_util import RqtRoscommUtil
00049 from rqt_topic.topic_widget import TopicWidget
00050 
00051 
00052 class MoveitWidget(QWidget):
00053     """#TODO: comment
00054     """
00055     # To be connected to PluginContainerWidget
00056     sig_sysmsg = None
00057     sig_param = Signal(bool, str)  # param name emitted
00058     sig_node = Signal(bool, str)  # param name emitted
00059 
00060     _SPLITTER_H = 'splitter_horizontal'
00061 
00062     def __init__(self, parent, plugin_context):
00063         """
00064         @type parent: MoveitMain
00065         """
00066 
00067         self._nodes_monitored = ['/move_group']
00068         self._selected_topics = [('/pointcloud', 'sensor_msgs/PointCloud'),
00069                                  ('/pointcloud2', 'sensor_msgs/PointCloud2'),
00070                                  ('/image', 'sensor_msgs/Image'),
00071                                  ('/camera_info', 'sensor_msgs/CameraInfo')]
00072         self._params_monitored = ['/robot_description',
00073                                   '/robot_description_semantic']
00074 
00075         super(MoveitWidget, self).__init__()
00076         self._parent = parent
00077         self._plugin_context = plugin_context
00078         self._refresh_rate = 5  # With default value
00079 
00080         self._rospack = rospkg.RosPack()
00081         ui_file = os.path.join(self._rospack.get_path('rqt_moveit'),
00082                                'resource', 'moveit_top.ui')
00083         loadUi(ui_file, self, {'TopicWidget': TopicWidget})
00084 
00085         # Custom widget classes don't show in QSplitter when they instantiated
00086         # in .ui file and not explicitly added to QSplitter like this. Thus
00087         # this is a workaround.
00088         self._splitter.addWidget(self._widget_topic)
00089 
00090         self._spinbox_refreshrate.valueChanged.connect(
00091                                                       self._update_refreshrate)
00092         # Show default ref rate on QSpinbox
00093         self._spinbox_refreshrate.setValue(self._refresh_rate)
00094 
00095         # Monitor node
00096         self._is_checking_nodes = True
00097         self._node_qitems = {}
00098         self._node_monitor_thread = self._init_monitor_nodes(
00099                                                          self._nodes_monitored)
00100         self._node_monitor_thread.start()
00101         #TODO: connect sys msg for nodes.
00102 
00103         # topic to show
00104         # Delegate GUI functionality to rqt_topic.TopicWidget.
00105         self._widget_topic.set_selected_topics(self._selected_topics)
00106         self._widget_topic.set_topic_specifier(TopicWidget.SELECT_BY_MSGTYPE)
00107         self._widget_topic.start()
00108         # To connect signal in a widget to PluginContainerWidget.
00109         #TODO: In this way, Signal from only one instance is hooked.
00110         # Not a good design at all.
00111         self.sig_sysmsg = self._widget_topic.sig_sysmsg
00112 
00113         # Init monitoring parameters.
00114         self._is_checking_params = True
00115         self._param_qitems = {}
00116         _col_names_paramtable = ['Param name', 'Found on Parameter Server?']
00117         self._param_check_thread = self._init_monitor_parameters(
00118                                                       self._params_monitored,
00119                                                       _col_names_paramtable)
00120         self._param_check_thread.start()
00121 
00122     def _init_monitor_nodes(self, nodes_monitored):
00123         """
00124         @type params_monitored: str[]
00125         @rtype: Thread
00126         """
00127         self._node_datamodel = QStandardItemModel(0, 2)
00128         self._root_qitem = self._node_datamodel.invisibleRootItem()
00129         self._view_nodes.setModel(self._node_datamodel)
00130 
00131         node_monitor_thread = Thread(target=self._check_nodes_alive,
00132                                            args=(self.sig_node,
00133                                                  self._nodes_monitored))
00134 #        self._node_monitor_thread = NodeMonitorThread(self, self.sig_node,
00135 #                                                      nodes_monitored)
00136         self.sig_node.connect(self._update_output_nodes)
00137         return node_monitor_thread
00138 
00139     def _check_nodes_alive(self, signal, nodes_monitored):
00140         """
00141         Working as a callback of Thread class, this method keeps looping to
00142         watch if the nodes whose names are passed exist and emits signal per
00143         each node.
00144 
00145         Notice that what MoveitWidget._check_nodes_alive &
00146         MoveitWidget._check_params_alive do is very similar, but since both of
00147         them are supposed to be passed to Thread class, there might not be
00148         a way to generalize these 2.
00149 
00150         @param signal: Signal(bool, str)
00151         @type nodes_monitored: str[]
00152         """
00153         while self._is_checking_nodes:
00154             rosnode_dynamically_loaded = __import__('rosnode')
00155             #from rosnode import rosnode_ping
00156             for nodename in nodes_monitored:
00157                 #TODO: rosnode_ping prints when the node is not found.
00158                 # Currently I have no idea how to capture that from here.
00159                 try:
00160                     #is_node_running = rosnode_ping(nodename, 1)
00161                     is_node_running = rosnode_dynamically_loaded.rosnode_ping(
00162                                                                  nodename, 1)
00163                 except rosnode_dynamically_loaded.ROSNodeIOException as e:
00164                     #TODO: Needs to be indicated on GUI
00165                     #      (eg. PluginContainerWidget)
00166                     rospy.logerr(e.message)
00167                     is_node_running = False
00168 
00169                 signal.emit(is_node_running, nodename)
00170                 rospy.logdebug('_update_output_nodes')
00171             del rosnode_dynamically_loaded
00172             time.sleep(self._refresh_rate)
00173 
00174     def _init_monitor_parameters(self, params_monitored,
00175                                  _col_names_paramtable=None):
00176         """
00177         @type params_monitored: str[]
00178         """
00179         self._params_monitored = params_monitored
00180 
00181         self._param_datamodel = QStandardItemModel(0, 2)
00182         self._root_qitem = self._param_datamodel.invisibleRootItem()
00183         self._view_params.setModel(self._param_datamodel)
00184 
00185         # Names of header on the QTableView.
00186         if not _col_names_paramtable:
00187             _col_names_paramtable = ['Param name',
00188                                      'Found on Parameter Server?']
00189         self._param_datamodel.setHorizontalHeaderLabels(_col_names_paramtable)
00190 
00191         self.sig_param.connect(self._update_output_parameters)
00192 
00193         param_check_thread = Thread(target=self._check_params_alive,
00194                                     args=(self.sig_param,
00195                                           self._params_monitored))
00196         return param_check_thread
00197 
00198     def _update_output_nodes(self, is_node_running, node_name):
00199         """
00200         Slot for signals that tell nodes existence.
00201 
00202         @type is_node_running: bool
00203         @type node_name: str
00204         """
00205         #TODO: What this method does is exactly the same with
00206         # monitor_parameters. Unify them.
00207 
00208         rospy.logdebug('is_node_running={} par_name={}'.format(is_node_running,
00209                                                               node_name))
00210         node_name = str(node_name)
00211         node_qitem = None
00212         if not node_name in self._node_qitems:
00213             node_qitem = QStandardItem(node_name)
00214             self._node_qitems[node_name] = node_qitem
00215             self._node_datamodel.appendRow(node_qitem)
00216         else:  # qsitem with the node name already exists.
00217             node_qitem = self._node_qitems[str(node_name)]
00218 
00219         qindex = self._node_datamodel.indexFromItem(node_qitem)
00220         _str_node_running = 'Not running'
00221         if is_node_running:
00222             _str_node_running = 'Running'
00223         qitem_node_status = QStandardItem(_str_node_running)
00224         self._node_datamodel.setItem(qindex.row(), 1, qitem_node_status)
00225 
00226     def _check_params_alive(self, signal, params_monitored):
00227         """
00228         Working as a callback of Thread class, this method keeps looping to
00229         watch if the params whose names are passed exist and emits signal per
00230         each node.
00231 
00232         Notice that what MoveitWidget._check_nodes_alive &
00233         MoveitWidget._check_params_alive do is very similar, but since both of
00234         them are supposed to be passed to Thread class, there might not be
00235         a way to generalize these 2.
00236 
00237         @type signal: Signal(bool, str)
00238         @param_name signal: emitting a name of the parameter that's found.
00239         @type params_monitored: str[]
00240         """
00241 
00242         while self._is_checking_params:
00243             # self._is_checking_params only turns to false when the plugin
00244             # shuts down.
00245 
00246             has_param = False
00247 
00248             for param_name in params_monitored:
00249                 is_rosmaster_running = RqtRoscommUtil.is_roscore_running()
00250 
00251                 try:
00252                     if is_rosmaster_running:
00253                         # Only if rosmaster is running, check if the parameter
00254                         # exists or not.
00255                         has_param = rospy.has_param(param_name)
00256                 except rospy.exceptions.ROSException as e:
00257                     self.sig_sysmsg.emit(
00258                          'Exception upon rospy.has_param {}'.format(e.message))
00259                 signal.emit(has_param, param_name)
00260                 rospy.loginfo('has_param {}, check_param_alive: {}'.format(
00261                                                       has_param, param_name))
00262             time.sleep(self._refresh_rate)
00263 
00264     def _update_output_parameters(self, has_param, param_name):
00265         """
00266         Slot
00267 
00268         @type has_param: bool
00269         @type param_name: str
00270         """
00271         rospy.logdebug('has_param={} par_name={}'.format(has_param,
00272                                                          param_name))
00273         param_name = str(param_name)
00274         param_qitem = None
00275         if not param_name in self._param_qitems:
00276             param_qitem = QStandardItem(param_name)
00277             self._param_qitems[param_name] = param_qitem
00278             self._param_datamodel.appendRow(param_qitem)
00279         else:  # qsitem with the param name already exists.
00280             param_qitem = self._param_qitems[str(param_name)]
00281 
00282         qindex = self._param_datamodel.indexFromItem(param_qitem)
00283         _str_param_found = 'No'
00284         if has_param:
00285             _str_param_found = 'Yes'
00286         qitem_param_status = QStandardItem(_str_param_found)
00287         self._param_datamodel.setItem(qindex.row(), 1, qitem_param_status)
00288         self._view_params.resizeColumnsToContents()
00289 
00290     def _update_refreshrate(self, refresh_rate):
00291         """
00292         Slot
00293 
00294         @type refresh_rate: int
00295         """
00296         self._refresh_rate = refresh_rate
00297 
00298     def save_settings(self, plugin_settings, instance_settings):
00299         instance_settings.set_value(self._SPLITTER_H,
00300                                     self._splitter.saveState())
00301 
00302     def restore_settings(self, plugin_settings, instance_settings):
00303         if instance_settings.contains(self._SPLITTER_H):
00304             self._splitter.restoreState(instance_settings.value(
00305                                                              self._SPLITTER_H))
00306         else:
00307             self._splitter.setSizes([100, 100, 200])
00308         pass
00309 
00310     def shutdown(self):
00311         """
00312         Overridden.
00313 
00314         Close threads.
00315 
00316         @raise RuntimeError:
00317         """
00318         try:
00319             #self._node_monitor_thread.join()  # No effect
00320             self._is_checking_nodes = False
00321             self._node_monitor_thread = None
00322             #self._param_check_thread.join()
00323 
00324             self._is_checking_params = False
00325             self._param_check_thread = None
00326         except RuntimeError as e:
00327             rospy.logerr(e)
00328             raise e
00329 
00330 
00331 if __name__ == '__main__':
00332     # main should be used only for debug purpose.
00333     # This moveites this QWidget as a standalone rqt gui.
00334     from rqt_gui.main import Main
00335 
00336     main = Main()
00337     sys.exit(main.main(sys.argv, standalone='rqt_moveit'))


rqt_moveit
Author(s): Isaac Saito
autogenerated on Mon Oct 6 2014 07:16:06