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 import threading
00038 import xmlrpclib
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
00043 from python_qt_binding.QtWidgets import QWidget
00044 import rospkg
00045 import rospy
00046 from rqt_py_common.rqt_roscomm_util import RqtRoscommUtil
00047 from rqt_topic.topic_widget import TopicWidget
00048 
00049 
00050 class MoveitWidget(QWidget):
00051     """
00052     This Widget provides an overview about the presence of different parts of a running moveIt instance.
00053     """
00054     # To be connected to PluginContainerWidget
00055     sig_sysmsg = Signal(str)
00056     sig_param = Signal(bool, str)  # param name emitted
00057     sig_node = Signal(bool, str)  # node name emitted
00058     sig_topic = Signal(list)  # topic 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._ros_master = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI'])
00068         self._stop_event = threading.Event()
00069 
00070         self._nodes_monitored = ['/move_group']
00071         self._selected_topics = [('/pointcloud', 'sensor_msgs/PointCloud'),
00072                                  ('/pointcloud2', 'sensor_msgs/PointCloud2'),
00073                                  ('/image', 'sensor_msgs/Image'),
00074                                  ('/camera_info', 'sensor_msgs/CameraInfo')]
00075         self._params_monitored = ['/robot_description',
00076                                   '/robot_description_semantic']
00077 
00078         super(MoveitWidget, self).__init__()
00079         self._parent = parent
00080         self._plugin_context = plugin_context
00081         self._refresh_rate = 5  # With default value
00082 
00083         self._rospack = rospkg.RosPack()
00084         ui_file = os.path.join(self._rospack.get_path('rqt_moveit'),
00085                                'resource', 'moveit_top.ui')
00086         loadUi(ui_file, self, {'TopicWidget': TopicWidget})
00087 
00088         # Custom widget classes don't show in QSplitter when they instantiated
00089         # in .ui file and not explicitly added to QSplitter like this. Thus
00090         # this is a workaround.
00091         self._splitter.addWidget(self._widget_topic)
00092 
00093         self._spinbox_refreshrate.valueChanged.connect(self._update_refreshrate)
00094 
00095         # Show default ref rate on QSpinbox
00096         self._spinbox_refreshrate.setValue(self._refresh_rate)
00097 
00098         # Monitor node
00099         self._node_qitems = {}
00100         self._node_monitor_thread = self._init_monitor_nodes()
00101         self._node_monitor_thread.start()
00102 
00103         # topic to show
00104         self._registered_topics = None
00105         self._topic_monitor_thread = self._init_monitor_topics()
00106         self._topic_monitor_thread.start()
00107 
00108         # Init monitoring parameters.
00109         self._param_qitems = {}
00110         _col_names_paramtable = ['Param name', 'Found on Parameter Server?']
00111         self._param_check_thread = self._init_monitor_parameters(_col_names_paramtable)
00112         self._param_check_thread.start()
00113 
00114     def _init_monitor_nodes(self):
00115         """
00116         @rtype: Thread
00117         """
00118         self._node_datamodel = QStandardItemModel(0, 2)
00119         self._root_qitem = self._node_datamodel.invisibleRootItem()
00120         self._view_nodes.setModel(self._node_datamodel)
00121 
00122         node_monitor_thread = threading.Thread(target=self._check_nodes_alive,
00123                                      args=(self.sig_node, self._nodes_monitored, self._stop_event))
00124 
00125         self.sig_node.connect(self._update_output_nodes)
00126         return node_monitor_thread
00127 
00128     def _check_nodes_alive(self, signal, nodes_monitored, stop_event):
00129         """
00130         Working as a callback of Thread class, this method keeps looping to
00131         watch if the nodes whose names are passed exist and emits signal per
00132         each node.
00133 
00134         Notice that what MoveitWidget._check_nodes_alive &
00135         MoveitWidget._check_params_alive do is very similar, but since both of
00136         them are supposed to be passed to Thread class, there might not be
00137         a way to generalize these 2.
00138 
00139         @param signal: Signal(bool, str)
00140         @type nodes_monitored: str[]
00141         @type stop_event: Event()
00142         """
00143         rosnode_dynamically_loaded = __import__('rosnode')
00144         while True:
00145             for nodename in nodes_monitored:
00146                 try:
00147                     registered_nodes = rosnode_dynamically_loaded.get_node_names()
00148                     is_node_running = nodename in registered_nodes
00149 
00150                 except rosnode_dynamically_loaded.ROSNodeIOException as e:
00151                     # TODO: Needs to be indicated on GUI
00152                     # (eg. PluginContainerWidget)
00153                     rospy.logerr(e.message)
00154                     is_node_running = False
00155 
00156                 signal.emit(is_node_running, nodename)
00157                 rospy.logdebug('_update_output_nodes')
00158             if stop_event.wait(self._refresh_rate):
00159                 del rosnode_dynamically_loaded
00160                 return
00161 
00162     def _init_monitor_topics(self):
00163         """
00164         @rtype: Thread
00165         """
00166         topic_monitor_thread = threading.Thread(target=self._check_topics_alive,
00167                                       args=(self.sig_topic, self._selected_topics, self._stop_event))
00168         self.sig_topic.connect(self._update_output_topics)
00169         return topic_monitor_thread
00170 
00171     def _check_topics_alive(self, signal, topics_monitored, stop_event):
00172         """
00173         Working as a callback of Thread class, this method keeps looping to
00174         watch if the topics whose names are passed exist and emits signal per
00175         each node.
00176 
00177         @param signal: Signal()
00178         @type topics_monitored: str[]
00179         @type stop_event: Event()
00180         """
00181         while True:
00182             code, msg, val = self._ros_master.getPublishedTopics('/rqt_moveit_update_script', "")
00183             if code == 1:
00184                 published_topics = dict(val)
00185             else:
00186                 rospy.logerr("Communication with rosmaster failed")
00187 
00188             registered_topics = []
00189             for topic in topics_monitored:
00190                 if topic[0] in published_topics and topic[1] == published_topics.get(topic[0]):
00191                     registered_topics.append((topic[0], published_topics.get(topic[0])))
00192 
00193             signal.emit(list(registered_topics))
00194             rospy.logdebug('_update_output_topics')
00195             if stop_event.wait(self._refresh_rate):
00196                 return
00197 
00198     def _init_monitor_parameters(self, _col_names_paramtable=None):
00199         """
00200         @rtype: Thread
00201         """
00202 
00203         self._param_datamodel = QStandardItemModel(0, 2)
00204         self._root_qitem = self._param_datamodel.invisibleRootItem()
00205         self._view_params.setModel(self._param_datamodel)
00206 
00207         # Names of header on the QTableView.
00208         if not _col_names_paramtable:
00209             _col_names_paramtable = ['Param name',
00210                                      'Found on Parameter Server?']
00211         self._param_datamodel.setHorizontalHeaderLabels(_col_names_paramtable)
00212 
00213         self.sig_param.connect(self._update_output_parameters)
00214 
00215         param_check_thread = threading.Thread(target=self._check_params_alive,
00216                                     args=(self.sig_param, self._params_monitored, self._stop_event))
00217         return param_check_thread
00218 
00219     def _update_output_nodes(self, is_node_running, node_name):
00220         """
00221         Slot for signals that tell nodes existence.
00222 
00223         @type is_node_running: bool
00224         @type node_name: str
00225         """
00226         # TODO: What this method does is exactly the same with
00227         # monitor_parameters. Unify them.
00228 
00229         rospy.logdebug('is_node_running={} par_name={}'.format(is_node_running, node_name))
00230         node_name = str(node_name)
00231         node_qitem = None
00232         if node_name not in self._node_qitems:
00233             node_qitem = QStandardItem(node_name)
00234             self._node_qitems[node_name] = node_qitem
00235             self._node_datamodel.appendRow(node_qitem)
00236         else:  # qsitem with the node name already exists.
00237             node_qitem = self._node_qitems[str(node_name)]
00238 
00239         qindex = self._node_datamodel.indexFromItem(node_qitem)
00240         _str_node_running = 'Not running'
00241         if is_node_running:
00242             _str_node_running = 'Running'
00243         qitem_node_status = QStandardItem(_str_node_running)
00244         self._node_datamodel.setItem(qindex.row(), 1, qitem_node_status)
00245 
00246     def _update_output_topics(self, registered_topics):
00247         """
00248         Slot for signals that tell topic's existence.
00249 
00250         @type registered_topics: list
00251         """
00252         # This branch will cause that once a selected topic was found the topic view will
00253         # never be empty again.
00254         if len(registered_topics) > 0:
00255 
00256             if self._registered_topics is None:
00257                 self._widget_topic.set_selected_topics(registered_topics)
00258                 self._widget_topic.set_topic_specifier(TopicWidget.SELECT_BY_NAME)
00259                 self._widget_topic.start()
00260             elif self._registered_topics is not None and set(self._registered_topics) != set(registered_topics):
00261                 self._widget_topic.set_selected_topics(registered_topics)
00262 
00263             self._registered_topics = registered_topics
00264 
00265     def _check_params_alive(self, signal, params_monitored, stop_event):
00266         """
00267         Working as a callback of Thread class, this method keeps looping to
00268         watch if the params whose names are passed exist and emits signal per
00269         each node.
00270 
00271         Notice that what MoveitWidget._check_nodes_alive &
00272         MoveitWidget._check_params_alive do is very similar, but since both of
00273         them are supposed to be passed to Thread class, there might not be
00274         a way to generalize these 2.
00275 
00276         @type signal: Signal(bool, str)
00277         @param_name signal: emitting a name of the parameter that's found.
00278         @type params_monitored: str[]
00279         @type stop_event: Event()
00280         """
00281 
00282         while True:
00283             has_param = False
00284 
00285             for param_name in params_monitored:
00286                 is_rosmaster_running = RqtRoscommUtil.is_roscore_running()
00287 
00288                 try:
00289                     if is_rosmaster_running:
00290                         # Only if rosmaster is running, check if the parameter
00291                         # exists or not.
00292                         has_param = rospy.has_param(param_name)
00293                 except rospy.exceptions.ROSException as e:
00294                     rospy.logerr('Exception upon rospy.has_param {}'.format(e.message))
00295                     self.sig_sysmsg.emit('Exception upon rospy.has_param {}'.format(e.message))
00296                 signal.emit(has_param, param_name)
00297                 rospy.logdebug('has_param {}, check_param_alive: {}'.format(has_param, param_name))
00298             if stop_event.wait(self._refresh_rate):
00299                 return
00300 
00301     def _update_output_parameters(self, has_param, param_name):
00302         """
00303         Slot
00304 
00305         @type has_param: bool
00306         @type param_name: str
00307         """
00308         rospy.logdebug('has_param={} par_name={}'.format(has_param,
00309                                                          param_name))
00310         param_name = str(param_name)
00311         param_qitem = None
00312         if param_name not in self._param_qitems:
00313             param_qitem = QStandardItem(param_name)
00314             self._param_qitems[param_name] = param_qitem
00315             self._param_datamodel.appendRow(param_qitem)
00316         else:  # qsitem with the param name already exists.
00317             param_qitem = self._param_qitems[str(param_name)]
00318 
00319         qindex = self._param_datamodel.indexFromItem(param_qitem)
00320         _str_param_found = 'No'
00321         if has_param:
00322             _str_param_found = 'Yes'
00323         qitem_param_status = QStandardItem(_str_param_found)
00324         self._param_datamodel.setItem(qindex.row(), 1, qitem_param_status)
00325         self._view_params.resizeColumnsToContents()
00326 
00327     def _update_refreshrate(self, refresh_rate):
00328         """
00329         Slot
00330 
00331         @type refresh_rate: int
00332         """
00333         self._refresh_rate = refresh_rate
00334 
00335     def save_settings(self, plugin_settings, instance_settings):
00336         instance_settings.set_value(self._SPLITTER_H,
00337                                     self._splitter.saveState())
00338 
00339     def restore_settings(self, plugin_settings, instance_settings):
00340         if instance_settings.contains(self._SPLITTER_H):
00341             self._splitter.restoreState(instance_settings.value(self._SPLITTER_H))
00342         else:
00343             self._splitter.setSizes([100, 100, 200])
00344         pass
00345 
00346     def shutdown(self):
00347         """
00348         Overridden.
00349 
00350         Close threads.
00351 
00352         @raise RuntimeError:
00353         """
00354         try:
00355             self._stop_event.set()
00356 
00357             self._node_monitor_thread.join()
00358             self._param_check_thread.join()
00359             self._topic_monitor_thread.join()
00360 
00361             self._node_monitor_thread = None
00362             self._param_check_thread = None
00363             self._topic_monitor_thread = None
00364 
00365         except RuntimeError as e:
00366             rospy.logerr(e)
00367             raise e
00368 
00369 
00370 if __name__ == '__main__':
00371     # main should be used only for debug purpose.
00372     # This moveites this QWidget as a standalone rqt gui.
00373     from rqt_gui.main import Main
00374 
00375     main = Main()
00376     sys.exit(main.main(sys.argv, standalone='rqt_moveit'))


rqt_moveit
Author(s): Isaac Saito
autogenerated on Thu Jun 6 2019 19:59:05