moveit_widget.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2012, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 #
33 # Author: Isaac Saito
34 
35 import os
36 import sys
37 import threading
38 import xmlrpclib
39 
40 from python_qt_binding import loadUi
41 from python_qt_binding.QtCore import QModelIndex, QTimer, Signal
42 from python_qt_binding.QtGui import QStandardItem, QStandardItemModel
43 from python_qt_binding.QtWidgets import QWidget
44 import rospkg
45 import rospy
46 from rqt_py_common.rqt_roscomm_util import RqtRoscommUtil
47 from rqt_topic.topic_widget import TopicWidget
48 
49 
50 class MoveitWidget(QWidget):
51  """
52  This Widget provides an overview about the presence of different parts of a running moveIt instance.
53  """
54  # To be connected to PluginContainerWidget
55  sig_sysmsg = Signal(str)
56  sig_param = Signal(bool, str) # param name emitted
57  sig_node = Signal(bool, str) # node name emitted
58  sig_topic = Signal(list) # topic name emitted
59 
60  _SPLITTER_H = 'splitter_horizontal'
61 
62  def __init__(self, parent, plugin_context):
63  """
64  @type parent: MoveitMain
65  """
66 
67  self._ros_master = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI'])
68  self._stop_event = threading.Event()
69 
70  self._nodes_monitored = ['/move_group']
71  self._selected_topics = [('/pointcloud', 'sensor_msgs/PointCloud'),
72  ('/pointcloud2', 'sensor_msgs/PointCloud2'),
73  ('/image', 'sensor_msgs/Image'),
74  ('/camera_info', 'sensor_msgs/CameraInfo')]
75  self._params_monitored = ['/robot_description',
76  '/robot_description_semantic']
77 
78  super(MoveitWidget, self).__init__()
79  self._parent = parent
80  self._plugin_context = plugin_context
81  self._refresh_rate = 5 # With default value
82 
83  self._rospack = rospkg.RosPack()
84  ui_file = os.path.join(self._rospack.get_path('rqt_moveit'),
85  'resource', 'moveit_top.ui')
86  loadUi(ui_file, self, {'TopicWidget': TopicWidget})
87 
88  # Custom widget classes don't show in QSplitter when they instantiated
89  # in .ui file and not explicitly added to QSplitter like this. Thus
90  # this is a workaround.
91  self._splitter.addWidget(self._widget_topic)
92 
93  self._spinbox_refreshrate.valueChanged.connect(self._update_refreshrate)
94 
95  # Show default ref rate on QSpinbox
96  self._spinbox_refreshrate.setValue(self._refresh_rate)
97 
98  # Monitor node
99  self._node_qitems = {}
101  self._node_monitor_thread.start()
102 
103  # topic to show
104  self._registered_topics = None
106  self._topic_monitor_thread.start()
107 
108  # Init monitoring parameters.
109  self._param_qitems = {}
110  _col_names_paramtable = ['Param name', 'Found on Parameter Server?']
111  self._param_check_thread = self._init_monitor_parameters(_col_names_paramtable)
112  self._param_check_thread.start()
113 
115  """
116  @rtype: Thread
117  """
118  self._node_datamodel = QStandardItemModel(0, 2)
119  self._root_qitem = self._node_datamodel.invisibleRootItem()
120  self._view_nodes.setModel(self._node_datamodel)
121 
122  node_monitor_thread = threading.Thread(target=self._check_nodes_alive,
123  args=(self.sig_node, self._nodes_monitored, self._stop_event))
124 
125  self.sig_node.connect(self._update_output_nodes)
126  return node_monitor_thread
127 
128  def _check_nodes_alive(self, signal, nodes_monitored, stop_event):
129  """
130  Working as a callback of Thread class, this method keeps looping to
131  watch if the nodes whose names are passed exist and emits signal per
132  each node.
133 
134  Notice that what MoveitWidget._check_nodes_alive &
135  MoveitWidget._check_params_alive do is very similar, but since both of
136  them are supposed to be passed to Thread class, there might not be
137  a way to generalize these 2.
138 
139  @param signal: Signal(bool, str)
140  @type nodes_monitored: str[]
141  @type stop_event: Event()
142  """
143  rosnode_dynamically_loaded = __import__('rosnode')
144  while True:
145  for nodename in nodes_monitored:
146  try:
147  registered_nodes = rosnode_dynamically_loaded.get_node_names()
148  is_node_running = nodename in registered_nodes
149 
150  except rosnode_dynamically_loaded.ROSNodeIOException as e:
151  # TODO: Needs to be indicated on GUI
152  # (eg. PluginContainerWidget)
153  rospy.logerr(e.message)
154  is_node_running = False
155 
156  signal.emit(is_node_running, nodename)
157  rospy.logdebug('_update_output_nodes')
158  if stop_event.wait(self._refresh_rate):
159  del rosnode_dynamically_loaded
160  return
161 
163  """
164  @rtype: Thread
165  """
166  topic_monitor_thread = threading.Thread(target=self._check_topics_alive,
167  args=(self.sig_topic, self._selected_topics, self._stop_event))
168  self.sig_topic.connect(self._update_output_topics)
169  return topic_monitor_thread
170 
171  def _check_topics_alive(self, signal, topics_monitored, stop_event):
172  """
173  Working as a callback of Thread class, this method keeps looping to
174  watch if the topics whose names are passed exist and emits signal per
175  each node.
176 
177  @param signal: Signal()
178  @type topics_monitored: str[]
179  @type stop_event: Event()
180  """
181  while True:
182  code, msg, val = self._ros_master.getPublishedTopics('/rqt_moveit_update_script', "")
183  if code == 1:
184  published_topics = dict(val)
185  else:
186  rospy.logerr("Communication with rosmaster failed")
187 
188  registered_topics = []
189  for topic in topics_monitored:
190  if topic[0] in published_topics and topic[1] == published_topics.get(topic[0]):
191  registered_topics.append((topic[0], published_topics.get(topic[0])))
192 
193  signal.emit(list(registered_topics))
194  rospy.logdebug('_update_output_topics')
195  if stop_event.wait(self._refresh_rate):
196  return
197 
198  def _init_monitor_parameters(self, _col_names_paramtable=None):
199  """
200  @rtype: Thread
201  """
202 
203  self._param_datamodel = QStandardItemModel(0, 2)
204  self._root_qitem = self._param_datamodel.invisibleRootItem()
205  self._view_params.setModel(self._param_datamodel)
206 
207  # Names of header on the QTableView.
208  if not _col_names_paramtable:
209  _col_names_paramtable = ['Param name',
210  'Found on Parameter Server?']
211  self._param_datamodel.setHorizontalHeaderLabels(_col_names_paramtable)
212 
213  self.sig_param.connect(self._update_output_parameters)
214 
215  param_check_thread = threading.Thread(target=self._check_params_alive,
216  args=(self.sig_param, self._params_monitored, self._stop_event))
217  return param_check_thread
218 
219  def _update_output_nodes(self, is_node_running, node_name):
220  """
221  Slot for signals that tell nodes existence.
222 
223  @type is_node_running: bool
224  @type node_name: str
225  """
226  # TODO: What this method does is exactly the same with
227  # monitor_parameters. Unify them.
228 
229  rospy.logdebug('is_node_running={} par_name={}'.format(is_node_running, node_name))
230  node_name = str(node_name)
231  node_qitem = None
232  if node_name not in self._node_qitems:
233  node_qitem = QStandardItem(node_name)
234  self._node_qitems[node_name] = node_qitem
235  self._node_datamodel.appendRow(node_qitem)
236  else: # qsitem with the node name already exists.
237  node_qitem = self._node_qitems[str(node_name)]
238 
239  qindex = self._node_datamodel.indexFromItem(node_qitem)
240  _str_node_running = 'Not running'
241  if is_node_running:
242  _str_node_running = 'Running'
243  qitem_node_status = QStandardItem(_str_node_running)
244  self._node_datamodel.setItem(qindex.row(), 1, qitem_node_status)
245 
246  def _update_output_topics(self, registered_topics):
247  """
248  Slot for signals that tell topic's existence.
249 
250  @type registered_topics: list
251  """
252  # This branch will cause that once a selected topic was found the topic view will
253  # never be empty again.
254  if len(registered_topics) > 0:
255 
256  if self._registered_topics is None:
257  self._widget_topic.set_selected_topics(registered_topics)
258  self._widget_topic.set_topic_specifier(TopicWidget.SELECT_BY_NAME)
259  self._widget_topic.start()
260  elif self._registered_topics is not None and set(self._registered_topics) != set(registered_topics):
261  self._widget_topic.set_selected_topics(registered_topics)
262 
263  self._registered_topics = registered_topics
264 
265  def _check_params_alive(self, signal, params_monitored, stop_event):
266  """
267  Working as a callback of Thread class, this method keeps looping to
268  watch if the params whose names are passed exist and emits signal per
269  each node.
270 
271  Notice that what MoveitWidget._check_nodes_alive &
272  MoveitWidget._check_params_alive do is very similar, but since both of
273  them are supposed to be passed to Thread class, there might not be
274  a way to generalize these 2.
275 
276  @type signal: Signal(bool, str)
277  @param_name signal: emitting a name of the parameter that's found.
278  @type params_monitored: str[]
279  @type stop_event: Event()
280  """
281 
282  while True:
283  has_param = False
284 
285  for param_name in params_monitored:
286  is_rosmaster_running = RqtRoscommUtil.is_roscore_running()
287 
288  try:
289  if is_rosmaster_running:
290  # Only if rosmaster is running, check if the parameter
291  # exists or not.
292  has_param = rospy.has_param(param_name)
293  except rospy.exceptions.ROSException as e:
294  rospy.logerr('Exception upon rospy.has_param {}'.format(e.message))
295  self.sig_sysmsg.emit('Exception upon rospy.has_param {}'.format(e.message))
296  signal.emit(has_param, param_name)
297  rospy.logdebug('has_param {}, check_param_alive: {}'.format(has_param, param_name))
298  if stop_event.wait(self._refresh_rate):
299  return
300 
301  def _update_output_parameters(self, has_param, param_name):
302  """
303  Slot
304 
305  @type has_param: bool
306  @type param_name: str
307  """
308  rospy.logdebug('has_param={} par_name={}'.format(has_param,
309  param_name))
310  param_name = str(param_name)
311  param_qitem = None
312  if param_name not in self._param_qitems:
313  param_qitem = QStandardItem(param_name)
314  self._param_qitems[param_name] = param_qitem
315  self._param_datamodel.appendRow(param_qitem)
316  else: # qsitem with the param name already exists.
317  param_qitem = self._param_qitems[str(param_name)]
318 
319  qindex = self._param_datamodel.indexFromItem(param_qitem)
320  _str_param_found = 'No'
321  if has_param:
322  _str_param_found = 'Yes'
323  qitem_param_status = QStandardItem(_str_param_found)
324  self._param_datamodel.setItem(qindex.row(), 1, qitem_param_status)
325  self._view_params.resizeColumnsToContents()
326 
327  def _update_refreshrate(self, refresh_rate):
328  """
329  Slot
330 
331  @type refresh_rate: int
332  """
333  self._refresh_rate = refresh_rate
334 
335  def save_settings(self, plugin_settings, instance_settings):
336  instance_settings.set_value(self._SPLITTER_H,
337  self._splitter.saveState())
338 
339  def restore_settings(self, plugin_settings, instance_settings):
340  if instance_settings.contains(self._SPLITTER_H):
341  self._splitter.restoreState(instance_settings.value(self._SPLITTER_H))
342  else:
343  self._splitter.setSizes([100, 100, 200])
344  pass
345 
346  def shutdown(self):
347  """
348  Overridden.
349 
350  Close threads.
351 
352  @raise RuntimeError:
353  """
354  try:
355  self._stop_event.set()
356 
357  self._node_monitor_thread.join()
358  self._param_check_thread.join()
359  self._topic_monitor_thread.join()
360 
361  self._node_monitor_thread = None
362  self._param_check_thread = None
363  self._topic_monitor_thread = None
364 
365  except RuntimeError as e:
366  rospy.logerr(e)
367  raise e
368 
369 
370 if __name__ == '__main__':
371  # main should be used only for debug purpose.
372  # This moveites this QWidget as a standalone rqt gui.
373  from rqt_gui.main import Main
374 
375  main = Main()
376  sys.exit(main.main(sys.argv, standalone='rqt_moveit'))
def _check_params_alive(self, signal, params_monitored, stop_event)
def save_settings(self, plugin_settings, instance_settings)
def _update_output_nodes(self, is_node_running, node_name)
def __init__(self, parent, plugin_context)
def _check_nodes_alive(self, signal, nodes_monitored, stop_event)
def _update_refreshrate(self, refresh_rate)
def _update_output_parameters(self, has_param, param_name)
def _init_monitor_parameters(self, _col_names_paramtable=None)
def restore_settings(self, plugin_settings, instance_settings)
def _update_output_topics(self, registered_topics)
def _check_topics_alive(self, signal, topics_monitored, stop_event)


rqt_moveit
Author(s): Isaac Saito
autogenerated on Wed Jun 5 2019 19:43:38