controller_manager.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Copyright (C) 2013, Georgia Tech
3 # Copyright (C) 2015, PAL Robotics S.L.
4 #
5 # Redistribution and use in source and binary forms, with or without
6 # modification, are permitted provided that the following conditions are met:
7 # * Redistributions of source code must retain the above copyright notice,
8 # this list of conditions and the following disclaimer.
9 # * Redistributions in binary form must reproduce the above copyright
10 # notice, this list of conditions and the following disclaimer in the
11 # documentation and/or other materials provided with the distribution.
12 # * Neither the name of PAL Robotics S.L. nor the names of its
13 # contributors may be used to endorse or promote products derived from
14 # this software without specific prior written permission.
15 #
16 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 # POSSIBILITY OF SUCH DAMAGE.
27 
28 import os
29 import rospkg
30 import rospy
31 
32 from python_qt_binding import loadUi
33 from python_qt_binding.QtCore import QAbstractTableModel, QModelIndex, Qt,\
34  QTimer, QVariant, Signal
35 from python_qt_binding.QtWidgets import QWidget, QFormLayout, QHeaderView,\
36  QMenu, QStyledItemDelegate
37 from python_qt_binding.QtGui import QCursor, QFont, QIcon, QStandardItem,\
38  QStandardItemModel
39 from qt_gui.plugin import Plugin
40 
41 from controller_manager_msgs.msg import ControllerState
42 from controller_manager_msgs.srv import *
43 from controller_manager_msgs.utils\
44  import ControllerLister, ControllerManagerLister,\
45  get_rosparam_controller_names
46 
47 from update_combo import update_combo
48 
49 
51  """
52  Graphical frontend for managing ros_control controllers.
53  """
54  _cm_update_freq = 1 # Hz
55 
56  def __init__(self, context):
57  super(ControllerManager, self).__init__(context)
58  self.setObjectName('ControllerManager')
59 
60  # Create QWidget and extend it with all the attributes and children
61  # from the UI file
62  self._widget = QWidget()
63  rp = rospkg.RosPack()
64  ui_file = os.path.join(rp.get_path('rqt_controller_manager'),
65  'resource',
66  'controller_manager.ui')
67  loadUi(ui_file, self._widget)
68  self._widget.setObjectName('ControllerManagerUi')
69 
70  # Pop-up that displays controller information
71  self._popup_widget = QWidget()
72  ui_file = os.path.join(rp.get_path('rqt_controller_manager'),
73  'resource',
74  'controller_info.ui')
75  loadUi(ui_file, self._popup_widget)
76  self._popup_widget.setObjectName('ControllerInfoUi')
77 
78  # Show _widget.windowTitle on left-top of each plugin (when
79  # it's set in _widget). This is useful when you open multiple
80  # plugins at once. Also if you open multiple instances of your
81  # plugin at once, these lines add number to make it easy to
82  # tell from pane to pane.
83  if context.serial_number() > 1:
84  self._widget.setWindowTitle(self._widget.windowTitle() +
85  (' (%d)' % context.serial_number()))
86  # Add widget to the user interface
87  context.add_widget(self._widget)
88 
89  # Initialize members
90  self._cm_ns = [] # Namespace of the selected controller manager
91  self._controllers = [] # State of each controller
92  self._table_model = None
93  self._controller_lister = None
94 
95  # Controller manager service proxies
96  self._load_srv = None
97  self._unload_srv = None
98  self._switch_srv = None
99 
100  # Controller state icons
101  rospack = rospkg.RosPack()
102  path = rospack.get_path('rqt_controller_manager')
103  self._icons = {'running': QIcon(path + '/resource/led_green.png'),
104  'stopped': QIcon(path + '/resource/led_red.png'),
105  'uninitialized': QIcon(path + '/resource/led_off.png')}
106 
107  # Controllers display
108  table_view = self._widget.table_view
109  table_view.setContextMenuPolicy(Qt.CustomContextMenu)
110  table_view.customContextMenuRequested.connect(self._on_ctrl_menu)
111 
112  table_view.doubleClicked.connect(self._on_ctrl_info)
113 
114  header = table_view.horizontalHeader()
115  header.setSectionResizeMode(QHeaderView.ResizeToContents)
116  header.setContextMenuPolicy(Qt.CustomContextMenu)
117  header.customContextMenuRequested.connect(self._on_header_menu)
118 
119  # Timer for controller manager updates
120  self._list_cm = ControllerManagerLister()
121  self._update_cm_list_timer = QTimer(self)
122  self._update_cm_list_timer.setInterval(1000.0 /
123  self._cm_update_freq)
124  self._update_cm_list_timer.timeout.connect(self._update_cm_list)
125  self._update_cm_list_timer.start()
126 
127  # Timer for running controller updates
128  self._update_ctrl_list_timer = QTimer(self)
129  self._update_ctrl_list_timer.setInterval(1000.0 /
130  self._cm_update_freq)
131  self._update_ctrl_list_timer.timeout.connect(self._update_controllers)
132  self._update_ctrl_list_timer.start()
133 
134  # Signal connections
135  w = self._widget
136  w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
137 
138  def shutdown_plugin(self):
139  self._update_cm_list_timer.stop()
140  self._update_ctrl_list_timer.stop()
141  self._popup_widget.hide()
142 
143  def save_settings(self, plugin_settings, instance_settings):
144  instance_settings.set_value('cm_ns', self._cm_ns)
145 
146  def restore_settings(self, plugin_settings, instance_settings):
147  # Restore last session's controller_manager, if present
148  self._update_cm_list()
149  cm_ns = instance_settings.value('cm_ns')
150  cm_combo = self._widget.cm_combo
151  cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
152  try:
153  idx = cm_list.index(cm_ns)
154  cm_combo.setCurrentIndex(idx)
155  except (ValueError):
156  pass
157 
158  # def trigger_configuration(self):
159  # Comment in to signal that the plugin has a way to configure
160  # This will enable a setting button (gear icon) in each dock widget
161  # title bar
162  # Usually used to open a modal configuration dialog
163 
164  def _update_cm_list(self):
165  update_combo(self._widget.cm_combo, self._list_cm())
166 
167  def _on_cm_change(self, cm_ns):
168  self._cm_ns = cm_ns
169 
170  # Setup services for communicating with the selected controller manager
171  self._set_cm_services(cm_ns)
172 
173  # Controller lister for the selected controller manager
174  if cm_ns:
175  self._controller_lister = ControllerLister(cm_ns)
176  self._update_controllers()
177  else:
178  self._controller_lister = None
179 
180  def _set_cm_services(self, cm_ns):
181  if cm_ns:
182  # NOTE: Persistent services are used for performance reasons.
183  # If the controller manager dies, we detect it and disconnect from
184  # it anyway
185  load_srv_name = _append_ns(cm_ns, 'load_controller')
186  self._load_srv = rospy.ServiceProxy(load_srv_name,
187  LoadController,
188  persistent=True)
189  unload_srv_name = _append_ns(cm_ns, 'unload_controller')
190  self._unload_srv = rospy.ServiceProxy(unload_srv_name,
191  UnloadController,
192  persistent=True)
193  switch_srv_name = _append_ns(cm_ns, 'switch_controller')
194  self._switch_srv = rospy.ServiceProxy(switch_srv_name,
195  SwitchController,
196  persistent=True)
197  else:
198  self._load_srv = None
199  self._unload_srv = None
200  self._switch_srv = None
201 
203  # Find controllers associated to the selected controller manager
204  controllers = self._list_controllers()
205 
206  # Update controller display, if necessary
207  if self._controllers != controllers:
208  self._controllers = controllers
209  self._show_controllers() # NOTE: Model is recomputed from scratch
210 
211  def _list_controllers(self):
212  """
213  @return List of controllers associated to a controller manager
214  namespace. Contains both stopped/running controllers, as returned by
215  the C{list_controllers} service, plus uninitialized controllers with
216  configurations loaded in the parameter server.
217  @rtype [str]
218  """
219  if not self._cm_ns:
220  return []
221 
222  # Add loaded controllers first
223  controllers = self._controller_lister()
224 
225  # Append potential controller configs found in the parameter server
226  all_ctrls_ns = _resolve_controllers_ns(self._cm_ns)
227  for name in get_rosparam_controller_names(all_ctrls_ns):
228  add_ctrl = not any(name == ctrl.name for ctrl in controllers)
229  if add_ctrl:
230  type_str = _rosparam_controller_type(all_ctrls_ns, name)
231  uninit_ctrl = ControllerState(name=name,
232  type=type_str,
233  state='uninitialized')
234  controllers.append(uninit_ctrl)
235  return controllers
236 
237  def _show_controllers(self):
238  table_view = self._widget.table_view
240  table_view.setModel(self._table_model)
241 
242  def _on_ctrl_menu(self, pos):
243  # Get data of selected controller
244  row = self._widget.table_view.rowAt(pos.y())
245  if row < 0:
246  return # Cursor is not under a valid item
247 
248  ctrl = self._controllers[row]
249 
250  # Show context menu
251  menu = QMenu(self._widget.table_view)
252  if ctrl.state == 'running':
253  action_stop = menu.addAction(self._icons['stopped'], 'Stop')
254  action_kill = menu.addAction(self._icons['uninitialized'],
255  'Stop and Unload')
256  elif ctrl.state == 'stopped':
257  action_start = menu.addAction(self._icons['running'], 'Start')
258  action_unload = menu.addAction(self._icons['uninitialized'],
259  'Unload')
260  elif ctrl.state == 'uninitialized':
261  action_load = menu.addAction(self._icons['stopped'], 'Load')
262  action_spawn = menu.addAction(self._icons['running'],
263  'Load and Start')
264 
265  action = menu.exec_(self._widget.table_view.mapToGlobal(pos))
266 
267  # Evaluate user action
268  if ctrl.state == 'running':
269  if action is action_stop:
270  self._stop_controller(ctrl.name)
271  elif action is action_kill:
272  self._stop_controller(ctrl.name)
273  self._unload_controller(ctrl.name)
274  elif ctrl.state == 'stopped':
275  if action is action_start:
276  self._start_controller(ctrl.name)
277  elif action is action_unload:
278  self._unload_controller(ctrl.name)
279  elif ctrl.state == 'uninitialized':
280  if action is action_load:
281  self._load_controller(ctrl.name)
282  if action is action_spawn:
283  self._load_controller(ctrl.name)
284  self._start_controller(ctrl.name)
285 
286  def _on_ctrl_info(self, index):
287  popup = self._popup_widget
288 
289  ctrl = self._controllers[index.row()]
290  popup.ctrl_name.setText(ctrl.name)
291  popup.ctrl_type.setText(ctrl.type)
292 
293  res_model = QStandardItemModel()
294  model_root = QStandardItem('Claimed Resources')
295  res_model.appendRow(model_root)
296  for hw_res in ctrl.claimed_resources:
297  hw_iface_item = QStandardItem(hw_res.hardware_interface)
298  model_root.appendRow(hw_iface_item)
299  for res in hw_res.resources:
300  res_item = QStandardItem(res)
301  hw_iface_item.appendRow(res_item)
302 
303  popup.resource_tree.setModel(res_model)
304  popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree))
305  popup.resource_tree.expandAll()
306  popup.move(QCursor.pos())
307  popup.show()
308 
309  def _on_header_menu(self, pos):
310  header = self._widget.table_view.horizontalHeader()
311 
312  # Show context menu
313  menu = QMenu(self._widget.table_view)
314  action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize')
315  action = menu.exec_(header.mapToGlobal(pos))
316 
317  # Evaluate user action
318  if action is action_toggle_auto_resize:
319  if header.resizeMode(0) == QHeaderView.ResizeToContents:
320  header.setSectionResizeMode(QHeaderView.Interactive)
321  else:
322  header.setSectionResizeMode(QHeaderView.ResizeToContents)
323 
324  def _load_controller(self, name):
325  self._load_srv.call(LoadControllerRequest(name=name))
326 
327  def _unload_controller(self, name):
328  self._unload_srv.call(UnloadControllerRequest(name=name))
329 
330  def _start_controller(self, name):
331  strict = SwitchControllerRequest.STRICT
332  req = SwitchControllerRequest(start_controllers=[name],
333  stop_controllers=[],
334  strictness=strict)
335  self._switch_srv.call(req)
336 
337  def _stop_controller(self, name):
338  strict = SwitchControllerRequest.STRICT
339  req = SwitchControllerRequest(start_controllers=[],
340  stop_controllers=[name],
341  strictness=strict)
342  self._switch_srv.call(req)
343 
344 
345 class ControllerTable(QAbstractTableModel):
346  """
347  Model containing controller information for tabular display.
348 
349  The model allows display of basic read-only information like controller
350  name and state.
351  """
352  def __init__(self, controller_info, icons, parent=None):
353  QAbstractTableModel.__init__(self, parent)
354  self._data = controller_info
355  self._icons = icons
356 
357  def rowCount(self, parent):
358  return len(self._data)
359 
360  def columnCount(self, parent):
361  return 2
362 
363  def headerData(self, col, orientation, role):
364  if orientation == Qt.Horizontal and role == Qt.DisplayRole:
365  if col == 0:
366  return 'controller'
367  elif col == 1:
368  return 'state'
369  else:
370  return None
371 
372  def data(self, index, role):
373  if not index.isValid():
374  return None
375 
376  ctrl = self._data[index.row()]
377 
378  if role == Qt.DisplayRole:
379  if index.column() == 0:
380  return ctrl.name
381  elif index.column() == 1:
382  return ctrl.state
383 
384  if role == Qt.DecorationRole:
385  if index.column() == 0:
386  return self._icons[ctrl.state]
387 
388  if role == Qt.FontRole:
389  if index.column() == 0:
390  bf = QFont()
391  bf.setBold(True)
392  return bf
393 
394  if role == Qt.TextAlignmentRole:
395  if index.column() == 1:
396  return Qt.AlignCenter
397 
398 
399 class FontDelegate(QStyledItemDelegate):
400  """
401  Simple delegate for customizing font weight and italization when
402  displaying resources claimed by a controller
403  """
404  def paint(self, painter, option, index):
405  if not index.parent().isValid():
406  # Root level
407  option.font.setWeight(QFont.Bold)
408  if index.parent().isValid() and not index.parent().parent().isValid():
409  # Hardware interface level
410  option.font.setItalic(True)
411  option.font.setWeight(QFont.Bold)
412  QStyledItemDelegate.paint(self, painter, option, index)
413 
414 
416  """
417  Resolve the namespace containing controller configurations from that of
418  the controller manager.
419  Controllers are assumed to live one level above the controller
420  manager, e.g.
421 
422  >>> _resolve_controller_ns('/path/to/controller_manager')
423  '/path/to'
424 
425  In the particular case in which the controller manager is not
426  namespaced, the controller is assumed to live in the root namespace
427 
428  >>> _resolve_controller_ns('/')
429  '/'
430  >>> _resolve_controller_ns('')
431  '/'
432  @param cm_ns Controller manager namespace (can be an empty string)
433  @type cm_ns str
434  @return Controllers namespace
435  @rtype str
436  """
437  ns = cm_ns.rsplit('/', 1)[0]
438  if not ns:
439  ns += '/'
440  return ns
441 
442 
443 def _append_ns(in_ns, suffix):
444  """
445  Append a sub-namespace (suffix) to the input namespace
446  @param in_ns Input namespace
447  @type in_ns str
448  @return Suffix namespace
449  @rtype str
450  """
451  ns = in_ns
452  if ns[-1] != '/':
453  ns += '/'
454  ns += suffix
455  return ns
456 
457 
458 def _rosparam_controller_type(ctrls_ns, ctrl_name):
459  """
460  Get a controller's type from its ROS parameter server configuration
461  @param ctrls_ns Namespace where controllers should be located
462  @type ctrls_ns str
463  @param ctrl_name Controller name
464  @type ctrl_name str
465  @return Controller type
466  @rtype str
467  """
468  type_param = _append_ns(ctrls_ns, ctrl_name) + '/type'
469  return rospy.get_param(type_param)
def __init__(self, controller_info, icons, parent=None)
def save_settings(self, plugin_settings, instance_settings)
def update_combo(combo, new_vals)
Definition: update_combo.py:28
def _rosparam_controller_type(ctrls_ns, ctrl_name)
def restore_settings(self, plugin_settings, instance_settings)


rqt_controller_manager
Author(s): Kelsey Hawkins , Adolfo Rodríguez Tsouroukdissian
autogenerated on Fri Jun 7 2019 22:00:16