def __init__(self, context):
super(Publisher, self).__init__(context)
self.setObjectName('Publisher')
# create widget
self._widget = PublisherWidget()
self._widget.add_publisher.connect(self.add_publisher)
self._widget.change_publisher.connect(self.change_publisher)
self._widget.publish_once.connect(self.publish_once)
self._widget.remove_publisher.connect(self.remove_publisher)
self._widget.clean_up_publishers.connect(self.clean_up_publishers)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# create context for the expression eval statement
self._eval_locals = {'i': 0}
for module in (math, random, time):
self._eval_locals.update(module.__dict__)
self._eval_locals['genpy'] = genpy
del self._eval_locals['__name__']
del self._eval_locals['__doc__']
self._publishers = {}
self._id_counter = 0
self._timeout_mapper = QSignalMapper(self)
self._timeout_mapper.mapped[int].connect(self.publish_once)
# add our self to the main window
context.add_widget(self._widget)
def set_menu(self, menu):
self._menu_manager = MenuManager(menu)
self._perspective_mapper = QSignalMapper(menu)
self._perspective_mapper.mapped[str].connect(self.switch_perspective)
# generate menu
create_action = QAction('&Create perspective...',
self._menu_manager.menu)
create_action.setIcon(QIcon.fromTheme('list-add'))
create_action.triggered.connect(self._on_create_perspective)
self._menu_manager.add_suffix(create_action)
self._remove_action = QAction('&Remove perspective...',
self._menu_manager.menu)
self._remove_action.setEnabled(False)
self._remove_action.setIcon(QIcon.fromTheme('list-remove'))
self._remove_action.triggered.connect(self._on_remove_perspective)
self._menu_manager.add_suffix(self._remove_action)
self._menu_manager.add_suffix(None)
import_action = QAction('&Import...', self._menu_manager.menu)
import_action.setIcon(QIcon.fromTheme('document-open'))
import_action.triggered.connect(self._on_import_perspective)
self._menu_manager.add_suffix(import_action)
export_action = QAction('&Export...', self._menu_manager.menu)
export_action.setIcon(QIcon.fromTheme('document-save-as'))
export_action.triggered.connect(self._on_export_perspective)
self._menu_manager.add_suffix(export_action)
# add perspectives to menu
for name in self.perspectives:
if not name.startswith(self.HIDDEN_PREFIX):
self._add_perspective_action(name)
class ROS_Publisher(QWidget):
def __init__(self):
super(ROS_Publisher, self).__init__()
self.id_counter = 0
self.publisher_dict = {}
self._timeout_mapper = QSignalMapper(self)
self._timeout_mapper.mapped[int].connect(self.publish_once)
def add_publisher(self, topic, type, rate, msg):
publisher = {}
publisher['publisher_id'] = self.id_counter
publisher['message'] = msg
publisher['publisher'] = rospy.Publisher(topic, type, queue_size=1)
self.publisher_dict[publisher['publisher_id']] = publisher
publisher['timer'] = QTimer(self)
self._timeout_mapper.setMapping(publisher['timer'],
publisher['publisher_id'])
publisher['timer'].timeout.connect(self._timeout_mapper.map)
publisher['timer'].start(int(1000.0 / rate))
self.id_counter += 1
def remove_publisher(self, id):
del self.publisher_dict[publisher['publisher_id']]
@Slot(int)
def publish_once(self, publisher_id):
publisher = self.publisher_dict.get(publisher_id, None)
if publisher is not None:
publisher['publisher'].publish(publisher['message'])
class MinimizedDockWidgetsToolbar(QToolBar):
max_label_length = 15
def __init__(self, container_manager, parent=None):
super(MinimizedDockWidgetsToolbar, self).__init__(parent=parent)
self.setWindowTitle(self.tr('Minimized dock widgets'))
self.setObjectName('MinimizedDockWidgetsToolbar')
self.setToolButtonStyle(Qt.ToolButtonTextBesideIcon)
self._container_manager = container_manager
self._signal_mapper = QSignalMapper(self)
self._signal_mapper.mapped[QWidget].connect(self._on_action_triggered)
self._dock_widgets = {}
self.hide()
def addDockWidget(self, dock_widget):
# remove action for same dock widget if exists
self.removeDockWidget(dock_widget)
icon = dock_widget.windowIcon()
if icon.isNull():
icon = QIcon.fromTheme('folder')
title = dock_widget.windowTitle()
action = QAction(icon, title, self)
# truncate label if necessary
if len(title) > MinimizedDockWidgetsToolbar.max_label_length:
action.setToolTip(title)
action.setIconText(
title[0:MinimizedDockWidgetsToolbar.max_label_length] + '...')
self._signal_mapper.setMapping(action, dock_widget)
action.triggered.connect(self._signal_mapper.map)
self._dock_widgets[dock_widget] = action
self.addAction(action)
self.show()
def removeDockWidget(self, dock_widget):
if dock_widget in self._dock_widgets:
action = self._dock_widgets[dock_widget]
self.removeAction(action)
del self._dock_widgets[dock_widget]
self._signal_mapper.removeMappings(action)
if not self._dock_widgets:
self.hide()
def _on_action_triggered(self, dock_widget):
# if the dock widget is nested inside a container also show the container
# do this recursively for nested containers
while True:
parent = self._container_manager.get_container_of_dock_widget(
dock_widget)
if parent is None:
break
dock_widget.show()
dock_widget = parent
dock_widget.show()
class MinimizedDockWidgetsToolbar(QToolBar):
max_label_length = 15
def __init__(self, container_manager, parent=None):
super(MinimizedDockWidgetsToolbar, self).__init__(parent=parent)
self.setWindowTitle(self.tr('Minimized dock widgets'))
self.setObjectName('MinimizedDockWidgetsToolbar')
self.setToolButtonStyle(Qt.ToolButtonTextBesideIcon)
self._container_manager = container_manager
self._signal_mapper = QSignalMapper(self)
self._signal_mapper.mapped[QWidget].connect(self._on_action_triggered)
self._dock_widgets = {}
self.hide()
def addDockWidget(self, dock_widget):
# remove action for same dock widget if exists
self.removeDockWidget(dock_widget)
icon = dock_widget.windowIcon()
if icon.isNull():
icon = QIcon.fromTheme('folder')
title = dock_widget.windowTitle()
action = QAction(icon, title, self)
# truncate label if necessary
if len(title) > MinimizedDockWidgetsToolbar.max_label_length:
action.setToolTip(title)
action.setIconText(title[0:MinimizedDockWidgetsToolbar.max_label_length] + '...')
self._signal_mapper.setMapping(action, dock_widget)
action.triggered.connect(self._signal_mapper.map)
self._dock_widgets[dock_widget] = action
self.addAction(action)
self.show()
def removeDockWidget(self, dock_widget):
if dock_widget in self._dock_widgets:
action = self._dock_widgets[dock_widget]
self.removeAction(action)
del self._dock_widgets[dock_widget]
self._signal_mapper.removeMappings(action)
if not self._dock_widgets:
self.hide()
def _on_action_triggered(self, dock_widget):
# if the dock widget is nested inside a container also show the container
# do this recursively for nested containers
while True:
parent = self._container_manager.get_container_of_dock_widget(dock_widget)
if parent is None:
break
dock_widget.show()
dock_widget = parent
dock_widget.show()
def __init__(self, container_manager, parent=None):
super(MinimizedDockWidgetsToolbar, self).__init__(parent=parent)
self.setWindowTitle(self.tr('Minimized dock widgets'))
self.setObjectName('MinimizedDockWidgetsToolbar')
self.setToolButtonStyle(Qt.ToolButtonTextBesideIcon)
self._container_manager = container_manager
self._signal_mapper = QSignalMapper(self)
self._signal_mapper.mapped[QWidget].connect(self._on_action_triggered)
self._dock_widgets = {}
self.hide()
def __init__(self, menu_bar, plugin_manager):
super(PluginMenu, self).__init__()
self.setObjectName('PluginMenu')
plugin_menu = menu_bar.addMenu(menu_bar.tr('Plugins'))
running_menu = menu_bar.addMenu(menu_bar.tr('Running'))
self._plugin_menu_manager = MenuManager(plugin_menu)
self._plugin_mapper = QSignalMapper(plugin_menu)
self._plugin_mapper.mapped[str].connect(self.load_plugin_signal)
self._running_menu_manager = MenuManager(running_menu)
self._running_mapper = QSignalMapper(running_menu)
self._running_mapper.mapped[str].connect(self.unload_plugin_signal)
self._instances = {}
def __init__(self, menu_bar, plugin_manager):
super(PluginMenu, self).__init__()
self.setObjectName('PluginMenu')
plugin_menu = menu_bar.addMenu(menu_bar.tr('&Plugins'))
running_menu = menu_bar.addMenu(menu_bar.tr('&Running'))
self._plugin_menu_manager = MenuManager(plugin_menu)
self._plugin_mapper = QSignalMapper(plugin_menu)
self._plugin_mapper.mapped[str].connect(self.load_plugin_signal)
self._running_menu_manager = MenuManager(running_menu)
self._running_mapper = QSignalMapper(running_menu)
self._running_mapper.mapped[str].connect(self.unload_plugin_signal)
self._instances = {}
def __init__(self, context):
super(Publisher, self).__init__(context)
self.setObjectName('Publisher')
# create widget
self._widget = PublisherWidget()
self._widget.add_publisher.connect(self.add_publisher)
self._widget.change_publisher.connect(self.change_publisher)
self._widget.publish_once.connect(self.publish_once)
self._widget.remove_publisher.connect(self.remove_publisher)
self._widget.clean_up_publishers.connect(self.clean_up_publishers)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# create context for the expression eval statement
self._eval_locals = {'i': 0}
for module in (math, random, time):
self._eval_locals.update(module.__dict__)
self._eval_locals['genpy'] = genpy
del self._eval_locals['__name__']
del self._eval_locals['__doc__']
self._publishers = {}
self._id_counter = 0
self._timeout_mapper = QSignalMapper(self)
self._timeout_mapper.mapped[int].connect(self.publish_once)
# add our self to the main window
context.add_widget(self._widget)
def set_menu(self, menu):
self._menu_manager = MenuManager(menu)
self._perspective_mapper = QSignalMapper(menu)
self._perspective_mapper.mapped[str].connect(self.switch_perspective)
# generate menu
create_action = QAction('&Create perspective...', self._menu_manager.menu)
create_action.setIcon(QIcon.fromTheme('list-add'))
create_action.triggered.connect(self._on_create_perspective)
self._menu_manager.add_suffix(create_action)
self._remove_action = QAction('&Remove perspective...', self._menu_manager.menu)
self._remove_action.setEnabled(False)
self._remove_action.setIcon(QIcon.fromTheme('list-remove'))
self._remove_action.triggered.connect(self._on_remove_perspective)
self._menu_manager.add_suffix(self._remove_action)
self._menu_manager.add_suffix(None)
import_action = QAction('&Import...', self._menu_manager.menu)
import_action.setIcon(QIcon.fromTheme('document-open'))
import_action.triggered.connect(self._on_import_perspective)
self._menu_manager.add_suffix(import_action)
export_action = QAction('&Export...', self._menu_manager.menu)
export_action.setIcon(QIcon.fromTheme('document-save-as'))
export_action.triggered.connect(self._on_export_perspective)
self._menu_manager.add_suffix(export_action)
# add perspectives to menu
for name in self.perspectives:
if not name.startswith(self.HIDDEN_PREFIX):
self._add_perspective_action(name)
def __init__(self, menu_bar, plugin_manager):
super(PluginMenu, self).__init__()
self.setObjectName('PluginMenu')
plugin_menu = menu_bar.addMenu(menu_bar.tr('&Plugins'))
running_menu = menu_bar.addMenu(menu_bar.tr('&Running'))
self._plugin_menu_manager = MenuManager(plugin_menu)
self._plugin_mapper = QSignalMapper(plugin_menu)
self._plugin_mapper.mapped[str].connect(self.load_plugin_signal)
self._running_menu_manager = MenuManager(running_menu)
action = QAction(' Hidden action to work around QTBUG-52582', self._running_menu_manager.menu)
action.setVisible(False)
self._running_menu_manager.add_item(action)
self._running_mapper = QSignalMapper(running_menu)
self._running_mapper.mapped[str].connect(self.unload_plugin_signal)
self._instances = {}
def __init__(self, container_manager, parent=None):
super(MinimizedDockWidgetsToolbar, self).__init__(parent=parent)
self.setWindowTitle(self.tr('Minimized dock widgets'))
self.setObjectName('MinimizedDockWidgetsToolbar')
self.setToolButtonStyle(Qt.ToolButtonTextBesideIcon)
self._container_manager = container_manager
self._signal_mapper = QSignalMapper(self)
self._signal_mapper.mapped[QWidget].connect(self._on_action_triggered)
self._dock_widgets = {}
self.hide()
def __init__(self, parent, main_window, instance_id, application_context,
container_manager, argv, dbus_object_path):
super(PluginHandlerXEmbedContainer,
self).__init__(parent, main_window, instance_id,
application_context, container_manager, argv)
self.setObjectName('PluginHandlerXEmbedContainer')
self._dbus_object_path = dbus_object_path
self._dbus_server = None
self._dbus_container_service = None
self._dbus_plugin_settings_service = None
self._dbus_instance_settings_service = None
self._process = None
self._pid = None
# mapping of widget object name to their embed container
self._embed_containers = {}
# mapping of toolbar object name to the toolbar
self._embed_toolbars = {}
self._signal_mapper_toolbars = QSignalMapper(self)
self._signal_mapper_toolbars.mapped[str].connect(
self._on_toolbar_orientation_changed)
def __init__(self, parent, main_window, instance_id, application_context, container_manager, argv, dbus_object_path):
super(PluginHandlerXEmbedContainer, self).__init__(parent, main_window, instance_id, application_context, container_manager, argv)
self.setObjectName('PluginHandlerXEmbedContainer')
self._dbus_object_path = dbus_object_path
self._dbus_server = None
self._dbus_container_service = None
self._dbus_plugin_settings_service = None
self._dbus_instance_settings_service = None
self._process = None
self._pid = None
# mapping of widget object name to their embed container
self._embed_containers = {}
# mapping of toolbar object name to the toolbar
self._embed_toolbars = {}
self._signal_mapper_toolbars = QSignalMapper(self)
self._signal_mapper_toolbars.mapped[str].connect(self._on_toolbar_orientation_changed)
class PluginMenu(QObject):
"""Menu of available plugins to load and running plugin instances to unload."""
load_plugin_signal = Signal(str)
unload_plugin_signal = Signal(str)
def __init__(self, menu_bar, plugin_manager):
super(PluginMenu, self).__init__()
self.setObjectName('PluginMenu')
plugin_menu = menu_bar.addMenu(menu_bar.tr('&Plugins'))
running_menu = menu_bar.addMenu(menu_bar.tr('&Running'))
self._plugin_menu_manager = MenuManager(plugin_menu)
self._plugin_mapper = QSignalMapper(plugin_menu)
self._plugin_mapper.mapped[str].connect(self.load_plugin_signal)
self._running_menu_manager = MenuManager(running_menu)
self._running_mapper = QSignalMapper(running_menu)
self._running_mapper.mapped[str].connect(self.unload_plugin_signal)
self._instances = {}
def add_plugin(self, plugin_descriptor):
base_path = plugin_descriptor.attributes().get('plugin_path')
menu_manager = self._plugin_menu_manager
# create submenus
for group in plugin_descriptor.groups():
label = group['label']
if menu_manager.contains_menu(label):
submenu = menu_manager.get_menu(label)
else:
submenu = QMenu(label, menu_manager.menu)
menu_action = submenu.menuAction()
self._enrich_action(menu_action, group, base_path)
menu_manager.add_item(submenu)
menu_manager = MenuManager(submenu)
# create action
action_attributes = plugin_descriptor.action_attributes()
action = QAction(action_attributes['label'], menu_manager.menu)
self._enrich_action(action, action_attributes, base_path)
self._plugin_mapper.setMapping(action, plugin_descriptor.plugin_id())
action.triggered.connect(self._plugin_mapper.map)
not_available = plugin_descriptor.attributes().get('not_available')
if not_available:
action.setEnabled(False)
action.setStatusTip(
self.tr('Plugin is not available: %s') % not_available)
# add action to menu
menu_manager.add_item(action)
def add_plugin_prefix(self, plugin_descriptor):
action_attributes = plugin_descriptor.action_attributes()
action = QAction(action_attributes['label'],
self._plugin_menu_manager.menu)
self._enrich_action(action, action_attributes)
self._plugin_mapper.setMapping(action, plugin_descriptor.plugin_id())
action.triggered.connect(self._plugin_mapper.map)
self._plugin_menu_manager.add_prefix(action)
def add_instance(self, plugin_descriptor, instance_id):
action_attributes = plugin_descriptor.action_attributes()
# create action
label = self.tr('Close:') + ' ' + action_attributes['label']
if instance_id.serial_number != 1:
label = label + ' (%s)' % str(instance_id.serial_number)
action = QAction(label, self._running_menu_manager.menu)
base_path = plugin_descriptor.attributes().get('plugin_path')
self._enrich_action(action, action_attributes, base_path)
self._running_mapper.setMapping(action, str(instance_id))
action.triggered.connect(self._running_mapper.map)
self._running_menu_manager.add_item(action)
self._instances[instance_id] = action
def remove_instance(self, instance_id):
action = self._instances[instance_id]
self._running_mapper.removeMappings(action)
self._running_menu_manager.remove_item(action)
def _enrich_action(self, action, action_attributes, base_path=None):
if 'icon' in action_attributes and action_attributes[
'icon'] is not None:
icon = get_icon(action_attributes['icon'],
action_attributes.get('icontype', None), base_path)
action.setIcon(icon)
if 'statustip' in action_attributes:
action.setStatusTip(action_attributes['statustip'])
class Publisher(Plugin):
def __init__(self, context):
super(Publisher, self).__init__(context)
self.setObjectName('Publisher')
# create widget
self._widget = PublisherWidget()
self._widget.add_publisher.connect(self.add_publisher)
self._widget.change_publisher.connect(self.change_publisher)
self._widget.publish_once.connect(self.publish_once)
self._widget.remove_publisher.connect(self.remove_publisher)
self._widget.clean_up_publishers.connect(self.clean_up_publishers)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# create context for the expression eval statement
self._eval_locals = {'i': 0}
for module in (math, random, time):
self._eval_locals.update(module.__dict__)
self._eval_locals['genpy'] = genpy
del self._eval_locals['__name__']
del self._eval_locals['__doc__']
self._publishers = {}
self._id_counter = 0
self._timeout_mapper = QSignalMapper(self)
self._timeout_mapper.mapped[int].connect(self.publish_once)
# add our self to the main window
context.add_widget(self._widget)
@Slot(str, str, float, bool)
def add_publisher(self, topic_name, type_name, rate, enabled):
publisher_info = {
'topic_name': str(topic_name),
'type_name': str(type_name),
'rate': float(rate),
'enabled': bool(enabled),
}
self._add_publisher(publisher_info)
def _add_publisher(self, publisher_info):
publisher_info['publisher_id'] = self._id_counter
self._id_counter += 1
publisher_info['counter'] = 0
publisher_info['enabled'] = publisher_info.get('enabled', False)
publisher_info['expressions'] = publisher_info.get('expressions', {})
publisher_info['message_instance'] = self._create_message_instance(publisher_info['type_name'])
if publisher_info['message_instance'] is None:
return
# create publisher and timer
try:
publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance']), queue_size=100)
except TypeError:
publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance']))
publisher_info['timer'] = QTimer(self)
# add publisher info to _publishers dict and create signal mapping
self._publishers[publisher_info['publisher_id']] = publisher_info
self._timeout_mapper.setMapping(publisher_info['timer'], publisher_info['publisher_id'])
publisher_info['timer'].timeout.connect(self._timeout_mapper.map)
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
self._widget.publisher_tree_widget.model().add_publisher(publisher_info)
@Slot(int, str, str, str, object)
def change_publisher(self, publisher_id, topic_name, column_name, new_value, setter_callback):
handler = getattr(self, '_change_publisher_%s' % column_name, None)
if handler is not None:
new_text = handler(self._publishers[publisher_id], topic_name, new_value)
if new_text is not None:
setter_callback(new_text)
def _change_publisher_topic(self, publisher_info, topic_name, new_value):
publisher_info['enabled'] = (new_value and new_value.lower() in ['1', 'true', 'yes'])
#qDebug('Publisher._change_publisher_enabled(): %s enabled: %s' % (publisher_info['topic_name'], publisher_info['enabled']))
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
else:
publisher_info['timer'].stop()
return None
def _change_publisher_type(self, publisher_info, topic_name, new_value):
type_name = new_value
# create new slot
slot_value = self._create_message_instance(type_name)
# find parent slot
slot_path = topic_name[len(publisher_info['topic_name']):].strip('/').split('/')
parent_slot = eval('.'.join(["publisher_info['message_instance']"] + slot_path[:-1]))
# find old slot
slot_name = slot_path[-1]
slot_index = parent_slot.__slots__.index(slot_name)
# restore type if user value was invalid
if slot_value is None:
qWarning('Publisher._change_publisher_type(): could not find type: %s' % (type_name))
return parent_slot._slot_types[slot_index]
else:
# replace old slot
parent_slot._slot_types[slot_index] = type_name
setattr(parent_slot, slot_name, slot_value)
self._widget.publisher_tree_widget.model().update_publisher(publisher_info)
def _change_publisher_rate(self, publisher_info, topic_name, new_value):
try:
rate = float(new_value)
except Exception:
qWarning('Publisher._change_publisher_rate(): could not parse rate value: %s' % (new_value))
else:
publisher_info['rate'] = rate
#qDebug('Publisher._change_publisher_rate(): %s rate changed: %fHz' % (publisher_info['topic_name'], publisher_info['rate']))
publisher_info['timer'].stop()
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
# make sure the column value reflects the actual rate
return '%.2f' % publisher_info['rate']
def _change_publisher_expression(self, publisher_info, topic_name, new_value):
expression = str(new_value)
if len(expression) == 0:
if topic_name in publisher_info['expressions']:
del publisher_info['expressions'][topic_name]
#qDebug('Publisher._change_publisher_expression(): removed expression for: %s' % (topic_name))
else:
slot_type, is_array = get_field_type(topic_name)
if is_array:
slot_type = list
# strip possible trailing error message from expression
error_prefix = '# error'
error_prefix_pos = expression.find(error_prefix)
if error_prefix_pos >= 0:
expression = expression[:error_prefix_pos]
success, _ = self._evaluate_expression(expression, slot_type)
if success:
old_expression = publisher_info['expressions'].get(topic_name, None)
publisher_info['expressions'][topic_name] = expression
#print 'Publisher._change_publisher_expression(): topic: %s, type: %s, expression: %s' % (topic_name, slot_type, new_value)
self._fill_message_slots(publisher_info['message_instance'], publisher_info['topic_name'], publisher_info['expressions'], publisher_info['counter'])
try:
publisher_info['message_instance']._check_types()
except Exception as e:
print('serialization error: %s' % e)
if old_expression is not None:
publisher_info['expressions'][topic_name] = old_expression
else:
del publisher_info['expressions'][topic_name]
return '%s %s: %s' % (expression, error_prefix, e)
return expression
else:
return '%s %s evaluating as "%s"' % (expression, error_prefix, slot_type.__name__)
def _extract_array_info(self, type_str):
array_size = None
if '[' in type_str and type_str[-1] == ']':
type_str, array_size_str = type_str.split('[', 1)
array_size_str = array_size_str[:-1]
if len(array_size_str) > 0:
array_size = int(array_size_str)
else:
array_size = 0
return type_str, array_size
def _create_message_instance(self, type_str):
base_type_str, array_size = self._extract_array_info(type_str)
base_message_type = roslib.message.get_message_class(base_type_str)
if base_message_type is None:
print('Could not create message of type "%s".' % base_type_str)
return None
if array_size is not None:
message = []
for _ in range(array_size):
message.append(base_message_type())
else:
message = base_message_type()
return message
def _evaluate_expression(self, expression, slot_type):
successful_eval = True
try:
# try to evaluate expression
value = eval(expression, {}, self._eval_locals)
except Exception:
successful_eval = False
if slot_type is str:
if successful_eval:
value = str(value)
else:
# for string slots just convert the expression to str, if it did not evaluate successfully
value = str(expression)
successful_eval = True
elif successful_eval:
type_set = set((slot_type, type(value)))
# check if value's type and slot_type belong to the same type group, i.e. array types, numeric types
# and if they do, make sure values's type is converted to the exact slot_type
if type_set <= set((list, tuple)) or type_set <= set((int, float)):
# convert to the right type
value = slot_type(value)
if successful_eval and isinstance(value, slot_type):
return True, value
else:
qWarning('Publisher._evaluate_expression(): failed to evaluate expression: "%s" as Python type "%s"' % (expression, slot_type.__name__))
return False, None
def _fill_message_slots(self, message, topic_name, expressions, counter):
if topic_name in expressions and len(expressions[topic_name]) > 0:
# get type
if hasattr(message, '_type'):
message_type = message._type
else:
message_type = type(message)
self._eval_locals['i'] = counter
success, value = self._evaluate_expression(expressions[topic_name], message_type)
if not success:
value = message_type()
return value
# if no expression exists for this topic_name, continue with it's child slots
elif hasattr(message, '__slots__'):
for slot_name in message.__slots__:
value = self._fill_message_slots(getattr(message, slot_name), topic_name + '/' + slot_name, expressions, counter)
if value is not None:
setattr(message, slot_name, value)
elif type(message) in (list, tuple) and (len(message) > 0):
for index, slot in enumerate(message):
value = self._fill_message_slots(slot, topic_name + '[%d]' % index, expressions, counter)
# this deals with primitive-type arrays
if not hasattr(message[0], '__slots__') and value is not None:
message[index] = value
return None
@Slot(int)
def publish_once(self, publisher_id):
publisher_info = self._publishers.get(publisher_id, None)
if publisher_info is not None:
publisher_info['counter'] += 1
self._fill_message_slots(publisher_info['message_instance'], publisher_info['topic_name'], publisher_info['expressions'], publisher_info['counter'])
publisher_info['publisher'].publish(publisher_info['message_instance'])
@Slot(int)
def remove_publisher(self, publisher_id):
publisher_info = self._publishers.get(publisher_id, None)
if publisher_info is not None:
publisher_info['timer'].stop()
publisher_info['publisher'].unregister()
del self._publishers[publisher_id]
def save_settings(self, plugin_settings, instance_settings):
publisher_copies = []
for publisher in self._publishers.values():
publisher_copy = {}
publisher_copy.update(publisher)
publisher_copy['enabled'] = False
del publisher_copy['timer']
del publisher_copy['message_instance']
del publisher_copy['publisher']
publisher_copies.append(publisher_copy)
instance_settings.set_value('publishers', repr(publisher_copies))
def restore_settings(self, plugin_settings, instance_settings):
publishers = eval(instance_settings.value('publishers', '[]'))
for publisher in publishers:
self._add_publisher(publisher)
def clean_up_publishers(self):
self._widget.publisher_tree_widget.model().clear()
for publisher_info in self._publishers.values():
publisher_info['timer'].stop()
publisher_info['publisher'].unregister()
self._publishers = {}
def shutdown_plugin(self):
self._widget.shutdown_plugin()
self.clean_up_publishers()
class PluginMenu(QObject):
"""Menu of available plugins to load and running plugin instances to unload."""
load_plugin_signal = Signal(str)
unload_plugin_signal = Signal(str)
def __init__(self, menu_bar, plugin_manager):
super(PluginMenu, self).__init__()
self.setObjectName('PluginMenu')
plugin_menu = menu_bar.addMenu(menu_bar.tr('&Plugins'))
running_menu = menu_bar.addMenu(menu_bar.tr('&Running'))
self._plugin_menu_manager = MenuManager(plugin_menu)
self._plugin_mapper = QSignalMapper(plugin_menu)
self._plugin_mapper.mapped[str].connect(self.load_plugin_signal)
self._running_menu_manager = MenuManager(running_menu)
self._running_mapper = QSignalMapper(running_menu)
self._running_mapper.mapped[str].connect(self.unload_plugin_signal)
self._instances = {}
def add_plugin(self, plugin_descriptor):
base_path = plugin_descriptor.attributes().get('plugin_path')
menu_manager = self._plugin_menu_manager
# create submenus
for group in plugin_descriptor.groups():
label = group['label']
if menu_manager.contains_menu(label):
submenu = menu_manager.get_menu(label)
else:
submenu = QMenu(label, menu_manager.menu)
menu_action = submenu.menuAction()
self._enrich_action(menu_action, group, base_path)
menu_manager.add_item(submenu)
menu_manager = MenuManager(submenu)
# create action
action_attributes = plugin_descriptor.action_attributes()
action = QAction(action_attributes['label'], menu_manager.menu)
self._enrich_action(action, action_attributes, base_path)
self._plugin_mapper.setMapping(action, plugin_descriptor.plugin_id())
action.triggered.connect(self._plugin_mapper.map)
not_available = plugin_descriptor.attributes().get('not_available')
if not_available:
action.setEnabled(False)
action.setStatusTip(self.tr('Plugin is not available: %s') % not_available)
# add action to menu
menu_manager.add_item(action)
def add_plugin_prefix(self, plugin_descriptor):
action_attributes = plugin_descriptor.action_attributes()
action = QAction(action_attributes['label'], self._plugin_menu_manager.menu)
self._enrich_action(action, action_attributes)
self._plugin_mapper.setMapping(action, plugin_descriptor.plugin_id())
action.triggered.connect(self._plugin_mapper.map)
self._plugin_menu_manager.add_prefix(action)
def add_instance(self, plugin_descriptor, instance_id):
action_attributes = plugin_descriptor.action_attributes()
action = QAction(self._get_instance_label(str(instance_id)), self._running_menu_manager.menu)
base_path = plugin_descriptor.attributes().get('plugin_path')
self._enrich_action(action, action_attributes, base_path)
self._running_mapper.setMapping(action, str(instance_id))
action.triggered.connect(self._running_mapper.map)
self._running_menu_manager.add_item(action)
self._instances[instance_id] = action
def remove_instance(self, instance_id):
action = self._instances[instance_id]
self._running_mapper.removeMappings(action)
self._running_menu_manager.remove_item(action)
@Slot(str, str)
def update_plugin_instance_label(self, instance_id_str, label):
instance_id = PluginInstanceId(instance_id=instance_id_str)
action = self._instances[instance_id]
action.setText(self._get_instance_label(label))
def _get_instance_label(self, label):
return self.tr('Close:') + ' ' + label
def _enrich_action(self, action, action_attributes, base_path=None):
if 'icon' in action_attributes and action_attributes['icon'] is not None:
icon = get_icon(action_attributes['icon'], action_attributes.get('icontype', None), base_path)
action.setIcon(icon)
if 'statustip' in action_attributes:
action.setStatusTip(action_attributes['statustip'])
class PluginMenu(QObject):
"""Menu of available plugins to load and running plugin instances to unload."""
load_plugin_signal = Signal(str)
unload_plugin_signal = Signal(str)
def __init__(self, menu_bar, plugin_manager):
super(PluginMenu, self).__init__()
self.setObjectName('PluginMenu')
plugin_menu = menu_bar.addMenu(menu_bar.tr('Plugins'))
running_menu = menu_bar.addMenu(menu_bar.tr('Running'))
self._plugin_menu_manager = MenuManager(plugin_menu)
self._plugin_mapper = QSignalMapper(plugin_menu)
self._plugin_mapper.mapped[str].connect(self.load_plugin_signal)
self._running_menu_manager = MenuManager(running_menu)
self._running_mapper = QSignalMapper(running_menu)
self._running_mapper.mapped[str].connect(self.unload_plugin_signal)
self._instances = {}
def add_plugin(self, plugin_descriptor):
base_path = plugin_descriptor.attributes().get('plugin_path')
menu_manager = self._plugin_menu_manager
# create submenus
for group in plugin_descriptor.groups():
label = group['label']
if menu_manager.contains_menu(label):
submenu = menu_manager.get_menu(label)
else:
submenu = QMenu(label, menu_manager.menu)
menu_action = submenu.menuAction()
self._enrich_action(menu_action, group, base_path)
menu_manager.add_item(submenu)
menu_manager = MenuManager(submenu)
# create action
action_attributes = plugin_descriptor.action_attributes()
action = QAction(action_attributes['label'], menu_manager.menu)
self._enrich_action(action, action_attributes, base_path)
self._plugin_mapper.setMapping(action, plugin_descriptor.plugin_id())
action.triggered.connect(self._plugin_mapper.map)
not_available = plugin_descriptor.attributes().get('not_available')
if not_available:
action.setEnabled(False)
action.setStatusTip(
self.tr('Plugin is not available: %s') % not_available)
# add action to menu
menu_manager.add_item(action)
def add_plugin_prefix(self, plugin_descriptor):
action_attributes = plugin_descriptor.action_attributes()
action = QAction(action_attributes['label'],
self._plugin_menu_manager.menu)
self._enrich_action(action, action_attributes)
self._plugin_mapper.setMapping(action, plugin_descriptor.plugin_id())
action.triggered.connect(self._plugin_mapper.map)
self._plugin_menu_manager.add_prefix(action)
def add_instance(self, plugin_descriptor, instance_id):
action_attributes = plugin_descriptor.action_attributes()
# create action
label = self.tr('Close:') + ' ' + action_attributes['label']
if instance_id.serial_number != 1:
label = label + ' (%s)' % str(instance_id.serial_number)
action = QAction(label, self._running_menu_manager.menu)
base_path = plugin_descriptor.attributes().get('plugin_path')
self._enrich_action(action, action_attributes, base_path)
self._running_mapper.setMapping(action, str(instance_id))
action.triggered.connect(self._running_mapper.map)
self._running_menu_manager.add_item(action)
self._instances[instance_id] = action
def remove_instance(self, instance_id):
action = self._instances[instance_id]
self._running_mapper.removeMappings(action)
self._running_menu_manager.remove_item(action)
def _enrich_action(self, action, action_attributes, base_path=None):
icontype = action_attributes.get('icontype', 'file')
if 'icon' in action_attributes and action_attributes[
'icon'] is not None:
if icontype == 'file':
path = action_attributes['icon']
if base_path is not None:
path = os.path.join(base_path, path)
icon = QIcon(path)
if len(icon.availableSizes()) == 0:
raise UserWarning('icon "%s" not found' % str(path))
elif icontype == 'resource':
icon = QIcon(action_attributes['icon'])
if len(icon.availableSizes()) == 0:
raise UserWarning('icon "%s" not found' % str(path))
elif icontype == 'theme':
# see http://standards.freedesktop.org/icon-naming-spec/icon-naming-spec-latest.html
icon = QIcon.fromTheme(action_attributes['icon'])
else:
raise UserWarning('unknown icon type "%s"' % str(icontype))
action.setIcon(icon)
if 'statustip' in action_attributes:
action.setStatusTip(action_attributes['statustip'])
def __init__(self, context):
super(PluginManagerWidget, self).__init__()
self.namespace = '/'
self.plugin_states_update_sub = None
self.load_plugin_set_client = None
self.get_plugin_descriptions_client = None
self.get_plugin_states_client = None
self.add_plugin_client = None
self.remove_plugin_client = None
self.plugin_descriptions = []
self.add_plugin_selection_filter = PluginDescription()
# start widget
widget = context
vbox = QVBoxLayout()
# load from ui
self.plugin_manager_widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('vigir_plugin_manager'), 'resource', 'plugin_manager.ui')
loadUi(ui_file, self.plugin_manager_widget, {'QWidget': QWidget})
vbox.addWidget(self.plugin_manager_widget)
# init ui
icon = QIcon.fromTheme("view-refresh")
self.plugin_manager_widget.refreshAllPushButton.setIcon(icon)
self.plugin_manager_widget.refreshPluginStatesPushButton.setIcon(icon)
# init tree view
tree_view = self.plugin_manager_widget.plugin_tree_view
tree_view.setSelectionMode(QAbstractItemView.ExtendedSelection)
tree_view.setContextMenuPolicy(Qt.CustomContextMenu)
tree_view.customContextMenuRequested.connect(self._open_context_menu)
self.plugin_tree_model = PluginTreeModel()
tree_view.setModel(self.plugin_tree_model)
# set up combo boxes
self.plugin_manager_widget.pluginNameComboBox.setInsertPolicy(QComboBox.NoInsert)
# references to combo boxes
self.plugin_cb = []
self.plugin_cb.append(self.plugin_manager_widget.pluginNameComboBox)
self.plugin_cb.append(self.plugin_manager_widget.pluginTypeClassComboBox)
self.plugin_cb.append(self.plugin_manager_widget.pluginTypePackageComboBox)
self.plugin_cb.append(self.plugin_manager_widget.pluginBaseClassComboBox)
self.plugin_cb.append(self.plugin_manager_widget.pluginBasePackageComboBox)
# init signal mapper
self.plugin_cb_mapper = QSignalMapper(self)
self.plugin_cb_mapper.mapped.connect(self.add_plugin_selection_changed)
# connect to signals
for i in range(len(self.plugin_cb)):
self.plugin_cb_mapper.setMapping(self.plugin_cb[i], i)
self.plugin_cb[i].currentIndexChanged.connect(self.plugin_cb_mapper.map)
self.plugin_manager_widget.namespaceComboBox.currentIndexChanged[str].connect(self.set_namespace)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(self.search_namespace)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(self.refresh_plugin_descriptions)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(self.refresh_plugin_states)
self.plugin_manager_widget.loadPluginSetPushButton.clicked[bool].connect(self.load_plugin_set)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[bool].connect(self.refresh_plugin_sets)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[bool].connect(self.refresh_plugin_descriptions)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[bool].connect(self.refresh_plugin_states)
self.plugin_manager_widget.addPluginPushButton.clicked[bool].connect(self.add_plugin)
self.plugin_manager_widget.clearAddPluginSelectionPushButton.clicked[bool].connect(self.clear_add_plugin_selection)
self.plugin_manager_widget.removePluginsPushButton.clicked[bool].connect(self.remove_plugins)
# Qt signals
self.plugin_states_updated_signal.connect(self.update_plugin_tree_view)
#self.connect(self, QtCore.SIGNAL('setTransitionModeStatusStyle(PyQt_PyObject)'), self._set_transition_mode_status_style)
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init plugin tree view
self.search_namespace()
class PluginHandlerXEmbedContainer(PluginHandler):
"""
Server part of the `PluginHandlerXEmbed`.
It starts the plugin in a subprocess and provides the `PluginHandlerDBusService` through a peer-to-peer DBus connection.
"""
_serial_number = 0
def __init__(self, parent, main_window, instance_id, application_context,
container_manager, argv, dbus_object_path):
super(PluginHandlerXEmbedContainer,
self).__init__(parent, main_window, instance_id,
application_context, container_manager, argv)
self.setObjectName('PluginHandlerXEmbedContainer')
self._dbus_object_path = dbus_object_path
self._dbus_server = None
self._dbus_container_service = None
self._dbus_plugin_settings_service = None
self._dbus_instance_settings_service = None
self._process = None
self._pid = None
# mapping of widget object name to their embed container
self._embed_containers = {}
# mapping of toolbar object name to the toolbar
self._embed_toolbars = {}
self._signal_mapper_toolbars = QSignalMapper(self)
self._signal_mapper_toolbars.mapped[str].connect(
self._on_toolbar_orientation_changed)
def _load(self):
self._dbus_server = Server('tcp:bind=*')
self._dbus_server.on_connection_added.append(self._add_dbus_connection)
self._dbus_container_service = PluginHandlerDBusService(
self, self._dbus_object_path)
self._dbus_plugin_settings_service = SettingsProxyDBusService(
self._dbus_object_path + '/plugin')
self._dbus_instance_settings_service = SettingsProxyDBusService(
self._dbus_object_path + '/instance')
self._process = QProcess(self)
self._process.setProcessChannelMode(QProcess.SeparateChannels)
self._process.readyReadStandardOutput.connect(
self._print_process_output)
self._process.readyReadStandardError.connect(self._print_process_error)
self._process.finished.connect(self._emit_close_plugin)
# start python with unbuffered stdout/stderr so that the order of the output is retained
cmd = sys.executable + ' -u'
cmd += ' %s' % Main.main_filename
cmd += ' --qt-binding=%s' % QT_BINDING
cmd += ' --embed-plugin=%s --embed-plugin-serial=%s --embed-plugin-address=%s' % (
self.instance_id().plugin_id, self.instance_id().serial_number,
self._dbus_server.address)
if self.argv():
cmd += ' --args %s' % ' '.join(self.argv())
#qDebug('PluginHandlerXEmbedContainer._load() starting command: %s' % cmd)
self._process.start(cmd)
started = self._process.waitForStarted(3000)
if not started:
self._dbus_container_service.remove_from_connection()
self._dbus_plugin_settings_service.remove_from_connection()
self._dbus_instance_settings_service.remove_from_connection()
raise RuntimeError(
'PluginHandlerXEmbedContainer._load() could not start subprocess in reasonable time'
)
# QProcess.pid() has been added to PySide in 1.0.5
if hasattr(self._process, 'pid'):
self._pid = self._process.pid()
else:
# use serial number as a replacement for pid if not available
self.__class__._serial_number = self._serial_number + 1
self._pid = self._serial_number
qDebug(
'PluginHandlerXEmbedContainer._load() started subprocess (#%s) for plugin "%s"'
% (self._pid, str(self._instance_id)))
# self._emit_load_completed is called asynchronous when client signals finished loading via dbus
def _add_dbus_connection(self, conn):
self._dbus_container_service.add_to_connection(conn,
self._dbus_object_path)
self._dbus_plugin_settings_service.add_to_connection(
conn, self._dbus_object_path + '/plugin')
self._dbus_instance_settings_service.add_to_connection(
conn, self._dbus_object_path + '/instance')
def _print_process_output(self):
self._print_process(self._process.readAllStandardOutput(), qDebug)
def _print_process_error(self):
self._print_process(self._process.readAllStandardError(), qWarning)
def _print_process(self, data, method):
# indent process output and prefix it with the pid
lines = str(data).split('\n')
if lines[-1] == '':
lines.pop()
for line in lines:
method(' %d %s' % (self._pid, line))
def load_completed(self, loaded, has_configuration):
# TODO timer to detect no response
exception = None if loaded else True
self._plugin_has_configuration = has_configuration
self._update_title_bars()
self._emit_load_completed(exception)
def _shutdown_plugin(self):
qDebug('PluginHandlerXEmbedContainer._shutdown_plugin()')
self._process.finished.disconnect(self._emit_close_plugin)
self._dbus_container_service.shutdown_plugin()
def emit_shutdown_plugin_completed(self):
self._dbus_container_service.remove_from_connection()
self._dbus_plugin_settings_service.remove_from_connection()
self._dbus_instance_settings_service.remove_from_connection()
self._process.close()
self._process.waitForFinished(5000)
if self._process.state() != QProcess.NotRunning:
self._process.kill()
self._process = None
super(PluginHandlerXEmbedContainer,
self).emit_shutdown_plugin_completed()
def _unload(self):
qDebug('PluginHandlerXEmbedContainer._unload()')
self._emit_unload_completed()
def _save_settings(self, plugin_settings, instance_settings):
qDebug('PluginHandlerXEmbedContainer._save_settings()')
self._dbus_plugin_settings_service.set_settings(plugin_settings)
self._dbus_instance_settings_service.set_settings(instance_settings)
self._dbus_container_service.save_settings()
def emit_save_settings_completed(self):
self._dbus_plugin_settings_service.set_settings(None)
self._dbus_instance_settings_service.set_settings(None)
super(PluginHandlerXEmbedContainer,
self).emit_save_settings_completed()
def _restore_settings(self, plugin_settings, instance_settings):
qDebug('PluginHandlerXEmbedContainer._restore_settings()')
self._dbus_plugin_settings_service.set_settings(plugin_settings)
self._dbus_instance_settings_service.set_settings(instance_settings)
self._dbus_container_service.restore_settings()
def emit_restore_settings_completed(self):
self._dbus_plugin_settings_service.set_settings(None)
self._dbus_instance_settings_service.set_settings(None)
super(PluginHandlerXEmbedContainer,
self).emit_restore_settings_completed()
def _trigger_configuration(self):
self._dbus_container_service.trigger_configuration()
def embed_widget(self, pid, widget_object_name):
dock_widget = self._create_dock_widget()
embed_container = QX11EmbedContainer(dock_widget)
#embed_container.clientClosed.connect(self._emit_close_signal)
self._add_dock_widget(dock_widget, embed_container)
# update widget title is triggered by client after embedding
self._embed_containers[widget_object_name] = embed_container
return embed_container.winId()
def update_embedded_widget_title(self, widget_object_name, title):
embed_container = self._embed_containers[widget_object_name]
embed_container.setWindowTitle(title)
def unembed_widget(self, widget_object_name):
embed_container = self._embed_containers[widget_object_name]
self.remove_widget(embed_container)
del self._embed_containers[widget_object_name]
def embed_toolbar(self, pid, toolbar_object_name):
toolbar = QToolBar()
toolbar.setObjectName(toolbar_object_name)
embed_container = QX11EmbedContainer(toolbar)
toolbar.addWidget(embed_container)
#embed_container.clientClosed.connect(self._emit_close_signal)
self._add_toolbar(toolbar)
self._embed_containers[toolbar_object_name] = embed_container
# setup mapping to signal change of orientation to client
self._embed_toolbars[toolbar_object_name] = toolbar
self._signal_mapper_toolbars.setMapping(toolbar, toolbar_object_name)
toolbar.orientationChanged.connect(self._signal_mapper_toolbars.map)
return embed_container.winId()
def _on_toolbar_orientation_changed(self, toolbar_object_name):
embed_container = self._embed_containers[toolbar_object_name]
toolbar = self._embed_toolbars[toolbar_object_name]
self._dbus_container_service.toolbar_orientation_changed(
embed_container.winId(),
toolbar.orientation() == Qt.Horizontal)
def unembed_toolbar(self, toolbar_object_name):
embed_container = self._embed_containers[toolbar_object_name]
del self._embed_containers[toolbar_object_name]
del self._embed_toolbars[toolbar_object_name]
self.remove_toolbar(embed_container)
embed_container.close()
class StepInterfaceWidget(QObject):
command_buttons = []
start_feet = Feet()
def __init__(self, context, add_execute_widget=True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left",
PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left",
PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up",
PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left",
PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward",
PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward",
PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over",
PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet",
PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance",
PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right",
PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right",
PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down",
PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right",
PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit,
"Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number,
"Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index,
"Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox,
self.start_foot_selection_combo_box,
"Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.0, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance,
"Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step,
"Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(
0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation,
"Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(
self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz,
"delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger=self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(
vbox,
QExecuteStepPlanWidget(logger=self.logger,
step_plan_topic="step_plan"),
"Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(
self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(
self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet,
self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan",
StepPlan,
queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient(
"step_plan_request", StepPlanRequestAction)
def shutdown_plugin(self):
print "Shutting down ..."
print "Done!"
def add_command_button(self, parent, text, command):
button = QPushButton(text)
self.command_mapper.setMapping(button, command)
button.clicked.connect(self.command_mapper.map)
parent.addWidget(button)
self.command_buttons.append(button)
return button
def set_start_feet_callback(self, feet):
self.start_feet = feet
# message publisher
def _publish_step_plan_request(self, walk_command):
params = PatternParameters()
params.steps = self.step_number.value()
params.mode = walk_command
params.close_step = self.close_step_checkbox.isChecked()
params.extra_seperation = self.extra_seperation_checkbox.isChecked()
params.use_terrain_model = self.use_terrain_model_checkbox.isChecked()
params.override = self.override_checkbox.isChecked(
) and not self.use_terrain_model_checkbox.isChecked()
params.roll = math.radians(self.roll.value())
params.pitch = math.radians(self.pitch.value())
params.dz = self.dz.value()
params.step_distance_forward = self.step_distance.value()
params.step_distance_sideward = self.side_step.value()
params.turn_angle = math.radians(self.step_rotation.value())
request = StepPlanRequest()
request.header = std_msgs.msg.Header()
request.header.stamp = rospy.Time.now()
request.header.frame_id = self.frame_id_line_edit.text()
request.start = self.start_feet
request.start_step_index = self.start_step_index.value()
if self.start_foot_selection_combo_box.currentText() == "AUTO":
request.start_foot_selection = StepPlanRequest.AUTO
elif self.start_foot_selection_combo_box.currentText() == "LEFT":
request.start_foot_selection = StepPlanRequest.LEFT
elif self.start_foot_selection_combo_box.currentText() == "RIGHT":
request.start_foot_selection = StepPlanRequest.RIGHT
else:
self.logger.log_error(
"Unknown start foot selection mode ('" +
self.start_foot_selection_combo_box.currentText() + "')!")
return
if walk_command == PatternParameters.FORWARD:
params.mode = PatternParameters.SAMPLING
elif walk_command == PatternParameters.BACKWARD:
params.mode = PatternParameters.SAMPLING
params.step_distance_forward *= -1
params.step_distance_sideward *= -1
params.turn_angle *= -1
request.pattern_parameters = params
request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
request.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name(
)
print "Send request = ", request
# send request
if self.step_plan_request_client.wait_for_server(rospy.Duration(0.5)):
self.logger.log_info("Sending footstep plan request...")
goal = StepPlanRequestGoal()
goal.plan_request = request
self.step_plan_request_client.send_goal(goal)
if self.step_plan_request_client.wait_for_result(
rospy.Duration(5.0)):
self.logger.log_info("Received footstep plan!")
self.logger.log(
self.step_plan_request_client.get_result().status)
self.step_plan_pub.publish(
self.step_plan_request_client.get_result().step_plan)
else:
self.logger.log_error(
"Didn't received any results. Check communcation!")
else:
self.logger.log_error(
"Can't connect to footstep planner action server!")
def commands_set_enabled(self, enable):
for button in self.command_buttons:
button.setEnabled(enable)
@Slot()
def param_cleared(self):
self.commands_set_enabled(False)
@Slot(str)
def param_selected(self, name):
self.commands_set_enabled(True)
@Slot(int)
def use_terrain_model_changed(self, state):
enable_override = True
if state == Qt.Checked:
enable_override = False
self.roll.setEnabled(enable_override)
self.pitch.setEnabled(enable_override)
self.override_checkbox.setEnabled(enable_override)
self.dz.setEnabled(enable_override)
def __init__(self, context):
super(PluginManagerWidget, self).__init__()
self.namespace = '/'
self.plugin_states_update_sub = None
self.load_plugin_set_client = None
self.get_plugin_descriptions_client = None
self.get_plugin_states_client = None
self.add_plugin_client = None
self.remove_plugin_client = None
self.plugin_descriptions = []
self.add_plugin_selection_filter = PluginDescription()
# start widget
widget = context
vbox = QVBoxLayout()
# load from ui
self.plugin_manager_widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('vigir_pluginlib_manager'),
'resource', 'plugin_manager.ui')
loadUi(ui_file, self.plugin_manager_widget, {'QWidget': QWidget})
vbox.addWidget(self.plugin_manager_widget)
# init ui
icon = QIcon.fromTheme("view-refresh")
self.plugin_manager_widget.refreshAllPushButton.setIcon(icon)
self.plugin_manager_widget.refreshPluginStatesPushButton.setIcon(icon)
# init tree view
tree_view = self.plugin_manager_widget.plugin_tree_view
tree_view.setSelectionMode(QAbstractItemView.ExtendedSelection)
tree_view.setContextMenuPolicy(Qt.CustomContextMenu)
tree_view.customContextMenuRequested.connect(self._open_context_menu)
self.plugin_tree_model = PluginTreeModel()
tree_view.setModel(self.plugin_tree_model)
# set up combo boxes
self.plugin_manager_widget.pluginNameComboBox.setInsertPolicy(
QComboBox.NoInsert)
# references to combo boxes
self.plugin_cb = []
self.plugin_cb.append(self.plugin_manager_widget.pluginNameComboBox)
self.plugin_cb.append(
self.plugin_manager_widget.pluginTypeClassComboBox)
self.plugin_cb.append(
self.plugin_manager_widget.pluginTypePackageComboBox)
self.plugin_cb.append(
self.plugin_manager_widget.pluginBaseClassComboBox)
self.plugin_cb.append(
self.plugin_manager_widget.pluginBasePackageComboBox)
# init signal mapper
self.plugin_cb_mapper = QSignalMapper(self)
self.plugin_cb_mapper.mapped.connect(self.add_plugin_selection_changed)
# connect to signals
for i in range(len(self.plugin_cb)):
self.plugin_cb_mapper.setMapping(self.plugin_cb[i], i)
self.plugin_cb[i].currentIndexChanged.connect(
self.plugin_cb_mapper.map)
self.plugin_manager_widget.namespaceComboBox.currentIndexChanged[
str].connect(self.set_namespace)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(
self.search_namespace)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(
self.refresh_plugin_descriptions)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(
self.refresh_plugin_states)
self.plugin_manager_widget.loadPluginSetPushButton.clicked[
bool].connect(self.load_plugin_set)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[
bool].connect(self.refresh_plugin_sets)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[
bool].connect(self.refresh_plugin_descriptions)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[
bool].connect(self.refresh_plugin_states)
self.plugin_manager_widget.addPluginPushButton.clicked[bool].connect(
self.add_plugin)
self.plugin_manager_widget.clearAddPluginSelectionPushButton.clicked[
bool].connect(self.clear_add_plugin_selection)
self.plugin_manager_widget.removePluginsPushButton.clicked[
bool].connect(self.remove_plugins)
# Qt signals
self.plugin_states_updated_signal.connect(self.update_plugin_tree_view)
#self.connect(self, QtCore.SIGNAL('setTransitionModeStatusStyle(PyQt_PyObject)'), self._set_transition_mode_status_style)
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init plugin tree view
self.search_namespace()
def __init__(self, context, add_execute_widget = True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left", PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left", PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up", PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left", PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward", PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward", PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over", PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet", PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance", PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right", PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right", PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down", PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right", PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit, "Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number, "Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index, "Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox, self.start_foot_selection_combo_box, "Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.20, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance, "Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step, "Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation, "Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz, "delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger = self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(vbox, QExecuteStepPlanWidget(logger = self.logger, step_plan_topic = "step_plan"), "Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet, self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan", StepPlan, queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient("step_plan_request", StepPlanRequestAction)
class StepInterfaceWidget(QObject):
command_buttons = []
start_feet = Feet()
def __init__(self, context, add_execute_widget = True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left", PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left", PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up", PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left", PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward", PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward", PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over", PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet", PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance", PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right", PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right", PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down", PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right", PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit, "Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number, "Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index, "Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox, self.start_foot_selection_combo_box, "Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.20, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance, "Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step, "Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation, "Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz, "delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger = self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(vbox, QExecuteStepPlanWidget(logger = self.logger, step_plan_topic = "step_plan"), "Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet, self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan", StepPlan, queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient("step_plan_request", StepPlanRequestAction)
def shutdown_plugin(self):
print "Shutting down ..."
print "Done!"
def add_command_button(self, parent, text, command):
button = QPushButton(text)
self.command_mapper.setMapping(button, command)
button.clicked.connect(self.command_mapper.map)
parent.addWidget(button)
self.command_buttons.append(button)
return button
def set_start_feet_callback(self, feet):
self.start_feet = feet
# message publisher
def _publish_step_plan_request(self, walk_command):
params = PatternParameters()
params.steps = self.step_number.value()
params.mode = walk_command
params.close_step = self.close_step_checkbox.isChecked()
params.extra_seperation = self.extra_seperation_checkbox.isChecked()
params.use_terrain_model = self.use_terrain_model_checkbox.isChecked()
params.override = self.override_checkbox.isChecked() and not self.use_terrain_model_checkbox.isChecked()
params.roll = math.radians(self.roll.value())
params.pitch = math.radians(self.pitch.value())
params.dz = self.dz.value()
params.step_distance_forward = self.step_distance.value()
params.step_distance_sideward = self.side_step.value()
params.turn_angle = math.radians(self.step_rotation.value())
request = StepPlanRequest()
request.header = std_msgs.msg.Header()
request.header.stamp = rospy.Time.now()
request.header.frame_id = self.frame_id_line_edit.text()
request.start = self.start_feet
request.start_step_index = self.start_step_index.value()
if (self.start_foot_selection_combo_box.currentText() == "AUTO"):
request.start_foot_selection = StepPlanRequest.AUTO
elif (self.start_foot_selection_combo_box.currentText() == "LEFT"):
request.start_foot_selection = StepPlanRequest.LEFT
elif (self.start_foot_selection_combo_box.currentText() == "RIGHT"):
request.start_foot_selection = StepPlanRequest.RIGHT
else:
self.logger.log_error("Unknown start foot selection mode ('" + self.start_foot_selection_combo_box.currentText() + "')!")
return;
if (walk_command == PatternParameters.FORWARD):
params.mode = PatternParameters.SAMPLING
elif (walk_command == PatternParameters.BACKWARD):
params.mode = PatternParameters.SAMPLING
params.step_distance_forward *= -1;
params.step_distance_sideward *= -1;
params.turn_angle *= -1;
request.pattern_parameters = params
request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
request.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name()
print "Send request = ", request
# send request
if (self.step_plan_request_client.wait_for_server(rospy.Duration(0.5))):
self.logger.log_info("Sending footstep plan request...")
goal = StepPlanRequestGoal()
goal.plan_request = request;
self.step_plan_request_client.send_goal(goal)
if (self.step_plan_request_client.wait_for_result(rospy.Duration(5.0))):
self.logger.log_info("Received footstep plan!")
self.logger.log(self.step_plan_request_client.get_result().status)
self.step_plan_pub.publish(self.step_plan_request_client.get_result().step_plan)
else:
self.logger.log_error("Didn't received any results. Check communcation!")
else:
self.logger.log_error("Can't connect to footstep planner action server!")
def commands_set_enabled(self, enable):
for button in self.command_buttons:
button.setEnabled(enable)
@Slot()
def param_cleared(self):
self.commands_set_enabled(False)
@Slot(str)
def param_selected(self, name):
self.commands_set_enabled(True)
@Slot(int)
def use_terrain_model_changed(self, state):
enable_override = True
if state == Qt.Checked:
enable_override = False
self.roll.setEnabled(enable_override)
self.pitch.setEnabled(enable_override)
self.override_checkbox.setEnabled(enable_override)
self.dz.setEnabled(enable_override)
class Publisher(Plugin):
def __init__(self, context):
super(Publisher, self).__init__(context)
self.setObjectName('Publisher')
# create widget
self._widget = PublisherWidget()
self._widget.add_publisher.connect(self.add_publisher)
self._widget.change_publisher.connect(self.change_publisher)
self._widget.publish_once.connect(self.publish_once)
self._widget.remove_publisher.connect(self.remove_publisher)
self._widget.clean_up_publishers.connect(self.clean_up_publishers)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# create context for the expression eval statement
self._eval_locals = {'i': 0}
for module in (math, random, time):
self._eval_locals.update(module.__dict__)
self._eval_locals['genpy'] = genpy
del self._eval_locals['__name__']
del self._eval_locals['__doc__']
self._publishers = {}
self._id_counter = 0
self._timeout_mapper = QSignalMapper(self)
self._timeout_mapper.mapped[int].connect(self.publish_once)
# add our self to the main window
context.add_widget(self._widget)
@Slot(str, str, float, bool)
def add_publisher(self, topic_name, type_name, rate, enabled):
publisher_info = {
'topic_name': str(topic_name),
'type_name': str(type_name),
'rate': float(rate),
'enabled': bool(enabled),
}
self._add_publisher(publisher_info)
def _add_publisher(self, publisher_info):
publisher_info['publisher_id'] = self._id_counter
self._id_counter += 1
publisher_info['counter'] = 0
publisher_info['enabled'] = publisher_info.get('enabled', False)
publisher_info['expressions'] = publisher_info.get('expressions', {})
publisher_info['message_instance'] = self._create_message_instance(
publisher_info['type_name'])
if publisher_info['message_instance'] is None:
return
# create publisher and timer
try:
publisher_info['publisher'] = rospy.Publisher(
publisher_info['topic_name'],
type(publisher_info['message_instance']),
queue_size=100)
except TypeError:
publisher_info['publisher'] = rospy.Publisher(
publisher_info['topic_name'],
type(publisher_info['message_instance']))
publisher_info['timer'] = QTimer(self)
# add publisher info to _publishers dict and create signal mapping
self._publishers[publisher_info['publisher_id']] = publisher_info
self._timeout_mapper.setMapping(publisher_info['timer'],
publisher_info['publisher_id'])
publisher_info['timer'].timeout.connect(self._timeout_mapper.map)
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
self._widget.publisher_tree_widget.model().add_publisher(
publisher_info)
@Slot(int, str, str, str, object)
def change_publisher(self, publisher_id, topic_name, column_name,
new_value, setter_callback):
handler = getattr(self, '_change_publisher_%s' % column_name, None)
if handler is not None:
new_text = handler(self._publishers[publisher_id], topic_name,
new_value)
if new_text is not None:
setter_callback(new_text)
def _change_publisher_topic(self, publisher_info, topic_name, new_value):
publisher_info['enabled'] = (new_value and new_value.lower()
in ['1', 'true', 'yes'])
#qDebug('Publisher._change_publisher_enabled(): %s enabled: %s' % (publisher_info['topic_name'], publisher_info['enabled']))
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
else:
publisher_info['timer'].stop()
return None
def _change_publisher_type(self, publisher_info, topic_name, new_value):
type_name = new_value
# create new slot
slot_value = self._create_message_instance(type_name)
# find parent slot
slot_path = topic_name[len(publisher_info['topic_name']):].strip(
'/').split('/')
parent_slot = eval('.'.join(["publisher_info['message_instance']"] +
slot_path[:-1]))
# find old slot
slot_name = slot_path[-1]
slot_index = parent_slot.__slots__.index(slot_name)
# restore type if user value was invalid
if slot_value is None:
qWarning(
'Publisher._change_publisher_type(): could not find type: %s' %
(type_name))
return parent_slot._slot_types[slot_index]
else:
# replace old slot
parent_slot._slot_types[slot_index] = type_name
setattr(parent_slot, slot_name, slot_value)
self._widget.publisher_tree_widget.model().update_publisher(
publisher_info)
def _change_publisher_rate(self, publisher_info, topic_name, new_value):
try:
rate = float(new_value)
except Exception:
qWarning(
'Publisher._change_publisher_rate(): could not parse rate value: %s'
% (new_value))
else:
publisher_info['rate'] = rate
#qDebug('Publisher._change_publisher_rate(): %s rate changed: %fHz' % (publisher_info['topic_name'], publisher_info['rate']))
publisher_info['timer'].stop()
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(
int(1000.0 / publisher_info['rate']))
# make sure the column value reflects the actual rate
return '%.2f' % publisher_info['rate']
def _change_publisher_expression(self, publisher_info, topic_name,
new_value):
expression = str(new_value)
if len(expression) == 0:
if topic_name in publisher_info['expressions']:
del publisher_info['expressions'][topic_name]
#qDebug('Publisher._change_publisher_expression(): removed expression for: %s' % (topic_name))
else:
slot_type, is_array = get_field_type(topic_name)
if is_array:
slot_type = list
# strip possible trailing error message from expression
error_prefix = '# error'
error_prefix_pos = expression.find(error_prefix)
if error_prefix_pos >= 0:
expression = expression[:error_prefix_pos]
success, _ = self._evaluate_expression(expression, slot_type)
if success:
old_expression = publisher_info['expressions'].get(
topic_name, None)
publisher_info['expressions'][topic_name] = expression
#print 'Publisher._change_publisher_expression(): topic: %s, type: %s, expression: %s' % (topic_name, slot_type, new_value)
self._fill_message_slots(publisher_info['message_instance'],
publisher_info['topic_name'],
publisher_info['expressions'],
publisher_info['counter'])
try:
publisher_info['message_instance']._check_types()
except Exception as e:
print('serialization error: %s' % e)
if old_expression is not None:
publisher_info['expressions'][
topic_name] = old_expression
else:
del publisher_info['expressions'][topic_name]
return '%s %s: %s' % (expression, error_prefix, e)
return expression
else:
return '%s %s evaluating as "%s"' % (expression, error_prefix,
slot_type.__name__)
def _extract_array_info(self, type_str):
array_size = None
if '[' in type_str and type_str[-1] == ']':
type_str, array_size_str = type_str.split('[', 1)
array_size_str = array_size_str[:-1]
if len(array_size_str) > 0:
array_size = int(array_size_str)
else:
array_size = 0
return type_str, array_size
def _create_message_instance(self, type_str):
base_type_str, array_size = self._extract_array_info(type_str)
base_message_type = roslib.message.get_message_class(base_type_str)
if base_message_type is None:
print('Could not create message of type "%s".' % base_type_str)
return None
if array_size is not None:
message = []
for _ in range(array_size):
message.append(base_message_type())
else:
message = base_message_type()
return message
def _evaluate_expression(self, expression, slot_type):
successful_eval = True
try:
# try to evaluate expression
value = eval(expression, {}, self._eval_locals)
except Exception:
successful_eval = False
if slot_type is str:
if successful_eval:
value = str(value)
else:
# for string slots just convert the expression to str, if it did not evaluate successfully
value = str(expression)
successful_eval = True
elif successful_eval:
type_set = set((slot_type, type(value)))
# check if value's type and slot_type belong to the same type group, i.e. array types, numeric types
# and if they do, make sure values's type is converted to the exact slot_type
if type_set <= set((list, tuple)) or type_set <= set((int, float)):
# convert to the right type
value = slot_type(value)
if successful_eval and isinstance(value, slot_type):
return True, value
else:
qWarning(
'Publisher._evaluate_expression(): failed to evaluate expression: "%s" as Python type "%s"'
% (expression, slot_type.__name__))
return False, None
def _fill_message_slots(self, message, topic_name, expressions, counter):
if topic_name in expressions and len(expressions[topic_name]) > 0:
# get type
if hasattr(message, '_type'):
message_type = message._type
else:
message_type = type(message)
self._eval_locals['i'] = counter
success, value = self._evaluate_expression(expressions[topic_name],
message_type)
if not success:
value = message_type()
return value
# if no expression exists for this topic_name, continue with it's child slots
elif hasattr(message, '__slots__'):
for slot_name in message.__slots__:
value = self._fill_message_slots(getattr(message, slot_name),
topic_name + '/' + slot_name,
expressions, counter)
if value is not None:
setattr(message, slot_name, value)
elif type(message) in (list, tuple) and (len(message) > 0):
for index, slot in enumerate(message):
value = self._fill_message_slots(slot,
topic_name + '[%d]' % index,
expressions, counter)
# this deals with primitive-type arrays
if not hasattr(message[0], '__slots__') and value is not None:
message[index] = value
return None
@Slot(int)
def publish_once(self, publisher_id):
publisher_info = self._publishers.get(publisher_id, None)
if publisher_info is not None:
publisher_info['counter'] += 1
self._fill_message_slots(publisher_info['message_instance'],
publisher_info['topic_name'],
publisher_info['expressions'],
publisher_info['counter'])
publisher_info['publisher'].publish(
publisher_info['message_instance'])
@Slot(int)
def remove_publisher(self, publisher_id):
publisher_info = self._publishers.get(publisher_id, None)
if publisher_info is not None:
publisher_info['timer'].stop()
publisher_info['publisher'].unregister()
del self._publishers[publisher_id]
def save_settings(self, plugin_settings, instance_settings):
publisher_copies = []
for publisher in self._publishers.values():
publisher_copy = {}
publisher_copy.update(publisher)
publisher_copy['enabled'] = False
del publisher_copy['timer']
del publisher_copy['message_instance']
del publisher_copy['publisher']
publisher_copies.append(publisher_copy)
instance_settings.set_value('publishers', repr(publisher_copies))
def restore_settings(self, plugin_settings, instance_settings):
publishers = eval(instance_settings.value('publishers', '[]'))
for publisher in publishers:
self._add_publisher(publisher)
def clean_up_publishers(self):
self._widget.publisher_tree_widget.model().clear()
for publisher_info in self._publishers.values():
publisher_info['timer'].stop()
publisher_info['publisher'].unregister()
self._publishers = {}
def shutdown_plugin(self):
self._widget.shutdown_plugin()
self.clean_up_publishers()
class PerspectiveManager(QObject):
"""Manager for perspectives associated with specific sets of `Settings`."""
perspective_changed_signal = Signal(basestring)
save_settings_signal = Signal(Settings, Settings)
restore_settings_signal = Signal(Settings, Settings)
restore_settings_without_plugin_changes_signal = Signal(Settings, Settings)
HIDDEN_PREFIX = '@'
def __init__(self, settings, application_context):
super(PerspectiveManager, self).__init__()
self.setObjectName('PerspectiveManager')
self._qtgui_path = application_context.qtgui_path
self._settings_proxy = SettingsProxy(settings)
self._global_settings = Settings(self._settings_proxy, 'global')
self._perspective_settings = None
self._create_perspective_dialog = None
self._menu_manager = None
self._perspective_mapper = None
# get perspective list from settings
self.perspectives = self._settings_proxy.value('', 'perspectives', [])
if isinstance(self.perspectives, basestring):
self.perspectives = [self.perspectives]
self._current_perspective = None
self._remove_action = None
self._callback = None
self._callback_args = []
if application_context.provide_app_dbus_interfaces:
from .perspective_manager_dbus_interface import PerspectiveManagerDBusInterface
self._dbus_server = PerspectiveManagerDBusInterface(self, application_context)
def set_menu(self, menu):
self._menu_manager = MenuManager(menu)
self._perspective_mapper = QSignalMapper(menu)
self._perspective_mapper.mapped[str].connect(self.switch_perspective)
# generate menu
create_action = QAction('&Create perspective...', self._menu_manager.menu)
create_action.setIcon(QIcon.fromTheme('list-add'))
create_action.triggered.connect(self._on_create_perspective)
self._menu_manager.add_suffix(create_action)
self._remove_action = QAction('&Remove perspective...', self._menu_manager.menu)
self._remove_action.setEnabled(False)
self._remove_action.setIcon(QIcon.fromTheme('list-remove'))
self._remove_action.triggered.connect(self._on_remove_perspective)
self._menu_manager.add_suffix(self._remove_action)
self._menu_manager.add_suffix(None)
import_action = QAction('&Import...', self._menu_manager.menu)
import_action.setIcon(QIcon.fromTheme('document-open'))
import_action.triggered.connect(self._on_import_perspective)
self._menu_manager.add_suffix(import_action)
export_action = QAction('&Export...', self._menu_manager.menu)
export_action.setIcon(QIcon.fromTheme('document-save-as'))
export_action.triggered.connect(self._on_export_perspective)
self._menu_manager.add_suffix(export_action)
# add perspectives to menu
for name in self.perspectives:
if not name.startswith(self.HIDDEN_PREFIX):
self._add_perspective_action(name)
def set_perspective(self, name, hide_and_without_plugin_changes=False):
if name is None:
name = self._settings_proxy.value('', 'current-perspective', 'Default')
elif hide_and_without_plugin_changes:
name = self.HIDDEN_PREFIX + name
self.switch_perspective(name, save_before=not hide_and_without_plugin_changes, without_plugin_changes=hide_and_without_plugin_changes)
@Slot(str)
@Slot(str, bool)
@Slot(str, bool, bool)
def switch_perspective(self, name, settings_changed=True, save_before=True, without_plugin_changes=False):
if save_before and self._global_settings is not None and self._perspective_settings is not None:
self._callback = self._switch_perspective
self._callback_args = [name, settings_changed, save_before]
self.save_settings_signal.emit(self._global_settings, self._perspective_settings)
else:
self._switch_perspective(name, settings_changed, save_before, without_plugin_changes)
def _switch_perspective(self, name, settings_changed, save_before, without_plugin_changes=False):
# convert from unicode
name = str(name.replace('/', '__'))
qDebug('PerspectiveManager.switch_perspective() switching to perspective "%s"' % name)
if self._current_perspective is not None and self._menu_manager is not None:
self._menu_manager.set_item_checked(self._current_perspective, False)
self._menu_manager.set_item_disabled(self._current_perspective, False)
# create perspective if necessary
if name not in self.perspectives:
self._create_perspective(name, clone_perspective=False)
# update current perspective
self._current_perspective = name
if self._menu_manager is not None:
self._menu_manager.set_item_checked(self._current_perspective, True)
self._menu_manager.set_item_disabled(self._current_perspective, True)
if not self._current_perspective.startswith(self.HIDDEN_PREFIX):
self._settings_proxy.set_value('', 'current-perspective', self._current_perspective)
self._perspective_settings = self._get_perspective_settings(self._current_perspective)
# emit signals
self.perspective_changed_signal.emit(self._current_perspective.lstrip(self.HIDDEN_PREFIX))
if settings_changed:
if not without_plugin_changes:
self.restore_settings_signal.emit(self._global_settings, self._perspective_settings)
else:
self.restore_settings_without_plugin_changes_signal.emit(self._global_settings, self._perspective_settings)
def save_settings_completed(self):
if self._callback is not None:
callback = self._callback
callback_args = self._callback_args
self._callback = None
self._callback_args = []
callback(*callback_args)
def _get_perspective_settings(self, perspective_name):
return Settings(self._settings_proxy, 'perspective/%s' % perspective_name)
def _on_create_perspective(self):
name = self._choose_new_perspective_name()
if name is not None:
clone_perspective = self._create_perspective_dialog.clone_checkbox.isChecked()
self._create_perspective(name, clone_perspective)
self.switch_perspective(name, settings_changed=not clone_perspective, save_before=False)
def _choose_new_perspective_name(self, show_cloning=True):
# input dialog for new perspective name
if self._create_perspective_dialog is None:
ui_file = os.path.join(self._qtgui_path, 'resource', 'perspective_create.ui')
self._create_perspective_dialog = loadUi(ui_file)
# custom validator preventing forward slashs
class CustomValidator(QValidator):
def __init__(self, parent=None):
super(CustomValidator, self).__init__(parent)
def fixup(self, value):
value = value.replace('/', '')
def validate(self, value, pos):
if value.find('/') != -1:
pos = value.find('/')
return (QValidator.Invalid, value, pos)
if value == '':
return (QValidator.Intermediate, value, pos)
return (QValidator.Acceptable, value, pos)
self._create_perspective_dialog.perspective_name_edit.setValidator(CustomValidator())
# set default values
self._create_perspective_dialog.perspective_name_edit.setText('')
self._create_perspective_dialog.clone_checkbox.setChecked(True)
self._create_perspective_dialog.clone_checkbox.setVisible(show_cloning)
# show dialog and wait for it's return value
return_value = self._create_perspective_dialog.exec_()
if return_value == self._create_perspective_dialog.Rejected:
return
name = str(self._create_perspective_dialog.perspective_name_edit.text()).lstrip(self.HIDDEN_PREFIX)
if name == '':
QMessageBox.warning(self._menu_manager.menu, self.tr('Empty perspective name'), self.tr('The name of the perspective must be non-empty.'))
return
if name in self.perspectives:
QMessageBox.warning(self._menu_manager.menu, self.tr('Duplicate perspective name'), self.tr('A perspective with the same name already exists.'))
return
return name
def _create_perspective(self, name, clone_perspective=True):
# convert from unicode
name = str(name)
if name.find('/') != -1:
raise RuntimeError('PerspectiveManager._create_perspective() name must not contain forward slashs (/)')
qDebug('PerspectiveManager._create_perspective(%s, %s)' % (name, clone_perspective))
# add to list of perspectives
self.perspectives.append(name)
self._settings_proxy.set_value('', 'perspectives', self.perspectives)
# save current settings
if self._global_settings is not None and self._perspective_settings is not None:
self._callback = self._create_perspective_continued
self._callback_args = [name, clone_perspective]
self.save_settings_signal.emit(self._global_settings, self._perspective_settings)
else:
self._create_perspective_continued(name, clone_perspective)
def _create_perspective_continued(self, name, clone_perspective):
# clone settings
if clone_perspective:
new_settings = self._get_perspective_settings(name)
keys = self._perspective_settings.all_keys()
for key in keys:
value = self._perspective_settings.value(key)
new_settings.set_value(key, value)
# add and switch to perspective
if not name.startswith(self.HIDDEN_PREFIX):
self._add_perspective_action(name)
def _add_perspective_action(self, name):
if self._menu_manager is not None:
# create action
action = QAction(name, self._menu_manager.menu)
action.setCheckable(True)
self._perspective_mapper.setMapping(action, name)
action.triggered.connect(self._perspective_mapper.map)
# add action to menu
self._menu_manager.add_item(action)
# enable remove-action
if self._menu_manager.count_items() > 1:
self._remove_action.setEnabled(True)
def _on_remove_perspective(self):
# input dialog to choose perspective to be removed
names = list(self.perspectives)
names.remove(self._current_perspective)
name, return_value = QInputDialog.getItem(self._menu_manager.menu, self._menu_manager.tr('Remove perspective'), self._menu_manager.tr('Select the perspective'), names, 0, False)
# convert from unicode
name = str(name)
if return_value == QInputDialog.Rejected:
return
self._remove_perspective(name)
def _remove_perspective(self, name):
if name not in self.perspectives:
raise UserWarning('unknown perspective: %s' % name)
qDebug('PerspectiveManager._remove_perspective(%s)' % str(name))
# remove from list of perspectives
self.perspectives.remove(name)
self._settings_proxy.set_value('', 'perspectives', self.perspectives)
# remove settings
settings = self._get_perspective_settings(name)
settings.remove('')
# remove from menu
self._menu_manager.remove_item(name)
# disable remove-action
if self._menu_manager.count_items() < 2:
self._remove_action.setEnabled(False)
def _on_import_perspective(self):
file_name, _ = QFileDialog.getOpenFileName(self._menu_manager.menu, self.tr('Import perspective from file'), None, self.tr('Perspectives (*.perspective)'))
if file_name is None or file_name == '':
return
perspective_name = os.path.basename(file_name)
suffix = '.perspective'
if perspective_name.endswith(suffix):
perspective_name = perspective_name[:-len(suffix)]
if perspective_name in self.perspectives:
perspective_name = self._choose_new_perspective_name(False)
if perspective_name is None:
return
self.import_perspective_from_file(file_name, perspective_name)
def import_perspective_from_file(self, path, perspective_name):
# create clean perspective
if perspective_name in self.perspectives:
self._remove_perspective(perspective_name)
self._create_perspective(perspective_name, clone_perspective=False)
# read perspective from file
file_handle = open(path, 'r')
#data = eval(file_handle.read())
data = json.loads(file_handle.read())
self._convert_values(data, self._import_value)
new_settings = self._get_perspective_settings(perspective_name)
self._set_dict_on_settings(data, new_settings)
self.switch_perspective(perspective_name, settings_changed=True, save_before=True)
def _set_dict_on_settings(self, data, settings):
"""Set dictionary key-value pairs on Settings instance."""
keys = data.get('keys', {})
for key in keys:
settings.set_value(key, keys[key])
groups = data.get('groups', {})
for group in groups:
sub = settings.get_settings(group)
self._set_dict_on_settings(groups[group], sub)
def _on_export_perspective(self):
file_name, _ = QFileDialog.getSaveFileName(self._menu_manager.menu, self.tr('Export perspective to file'), self._current_perspective + '.perspective', self.tr('Perspectives (*.perspective)'))
if file_name is None or file_name == '':
return
# trigger save of perspective before export
self._callback = self._on_export_perspective_continued
self._callback_args = [file_name]
self.save_settings_signal.emit(self._global_settings, self._perspective_settings)
def _on_export_perspective_continued(self, file_name):
# convert every value
data = self._get_dict_from_settings(self._perspective_settings)
self._convert_values(data, self._export_value)
# write perspective data to file
file_handle = open(file_name, 'w')
file_handle.write(json.dumps(data, indent=2))
file_handle.close()
def _get_dict_from_settings(self, settings):
"""Convert data of Settings instance to dictionary."""
keys = {}
for key in settings.child_keys():
keys[str(key)] = settings.value(key)
groups = {}
for group in settings.child_groups():
sub = settings.get_settings(group)
groups[str(group)] = self._get_dict_from_settings(sub)
return {'keys': keys, 'groups': groups}
def _convert_values(self, data, convert_function):
keys = data.get('keys', {})
for key in keys:
keys[key] = convert_function(keys[key])
groups = data.get('groups', {})
for group in groups:
self._convert_values(groups[group], convert_function)
def _import_value(self, value):
import QtCore # @UnusedImport
if value['type'] == 'repr':
return eval(value['repr'])
elif value['type'] == 'repr(QByteArray.hex)':
return QByteArray.fromHex(eval(value['repr(QByteArray.hex)']))
raise RuntimeError('PerspectiveManager._import_value() unknown serialization type (%s)' % value['type'])
def _export_value(self, value):
data = {}
if value.__class__.__name__ == 'QByteArray':
hex_value = value.toHex()
data['repr(QByteArray.hex)'] = self._strip_qt_binding_prefix(hex_value, repr(hex_value))
data['type'] = 'repr(QByteArray.hex)'
# add pretty print for better readability
characters = ''
for i in range(1, value.size(), 2):
character = value.at(i)
# output all non-control characters
if character >= ' ' and character <= '~':
characters += character
else:
characters += ' '
data['pretty-print'] = characters
else:
data['repr'] = self._strip_qt_binding_prefix(value, repr(value))
data['type'] = 'repr'
# verify that serialized data can be deserialized correctly
reimported = self._import_value(data)
if reimported != value:
raise RuntimeError('PerspectiveManager._export_value() stored value can not be restored (%s)' % type(value))
return data
def _strip_qt_binding_prefix(self, obj, data):
"""Strip binding specific prefix from type string."""
parts = obj.__class__.__module__.split('.')
if len(parts) > 1 and parts[1] == 'QtCore':
prefix = '.'.join(parts[:2])
data = data.replace(prefix, 'QtCore', 1)
return data
class Publisher(Plugin):
def __init__(self, context):
super(Publisher, self).__init__(context)
self.setObjectName('Publisher')
# create widget
self._widget = PublisherWidget()
self._widget.add_publisher.connect(self.add_publisher)
self._widget.change_publisher.connect(self.change_publisher)
self._widget.publish_once.connect(self.publish_once)
self._widget.remove_publisher.connect(self.remove_publisher)
self._widget.clean_up_publishers.connect(self.clean_up_publishers)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# create context for the expression eval statement
self._eval_locals = {'i': 0}
for module in (math, random, time):
self._eval_locals.update(module.__dict__)
self._eval_locals['genpy'] = genpy
del self._eval_locals['__name__']
del self._eval_locals['__doc__']
self._publishers = {}
self._id_counter = 0
self._timeout_mapper = QSignalMapper(self)
self._timeout_mapper.mapped[int].connect(self.publish_once)
# add our self to the main window
context.add_widget(self._widget)
@Slot(str, str, float, bool)
def add_publisher(self, topic_name, type_name, rate, enabled):
publisher_info = {
'topic_name': str(topic_name),
'type_name': str(type_name),
'rate': float(rate),
'enabled': bool(enabled),
}
self._add_publisher(publisher_info)
def _add_publisher(self, publisher_info):
publisher_info['publisher_id'] = self._id_counter
self._id_counter += 1
publisher_info['counter'] = 0
publisher_info['enabled'] = publisher_info.get('enabled', False)
publisher_info['expressions'] = publisher_info.get('expressions', {})
publisher_info['message_instance'] = self._create_message_instance(publisher_info['type_name'])
if publisher_info['message_instance'] is None:
return
# create publisher and timer
try:
publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance']), queue_size=100)
except TypeError:
publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance']))
publisher_info['timer'] = QTimer(self)
# add publisher info to _publishers dict and create signal mapping
self._publishers[publisher_info['publisher_id']] = publisher_info
self._timeout_mapper.setMapping(publisher_info['timer'], publisher_info['publisher_id'])
publisher_info['timer'].timeout.connect(self._timeout_mapper.map)
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
self._widget.publisher_tree_widget.model().add_publisher(publisher_info)
@Slot(int, str, str, str, object)
def change_publisher(self, publisher_id, topic_name, column_name, new_value, setter_callback):
handler = getattr(self, '_change_publisher_%s' % column_name, None)
if handler is not None:
new_text = handler(self._publishers[publisher_id], topic_name, new_value)
if new_text is not None:
setter_callback(new_text)
def _change_publisher_topic(self, publisher_info, topic_name, new_value):
publisher_info['enabled'] = (new_value and new_value.lower() in ['1', 'true', 'yes'])
#qDebug('Publisher._change_publisher_enabled(): %s enabled: %s' % (publisher_info['topic_name'], publisher_info['enabled']))
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
else:
publisher_info['timer'].stop()
return None
def _change_publisher_type(self, publisher_info, topic_name, new_value):
type_name = new_value
# create new slot
slot_value = self._create_message_instance(type_name)
# find parent slot
slot_path = topic_name[len(publisher_info['topic_name']):].strip('/').split('/')
parent_slot = eval('.'.join(["publisher_info['message_instance']"] + slot_path[:-1]))
# find old slot
slot_name = slot_path[-1]
slot_index = parent_slot.__slots__.index(slot_name)
# restore type if user value was invalid
if slot_value is None:
qWarning('Publisher._change_publisher_type(): could not find type: %s' % (type_name))
return parent_slot._slot_types[slot_index]
else:
# replace old slot
parent_slot._slot_types[slot_index] = type_name
setattr(parent_slot, slot_name, slot_value)
self._widget.publisher_tree_widget.model().update_publisher(publisher_info)
def _change_publisher_rate(self, publisher_info, topic_name, new_value):
try:
rate = float(new_value)
except Exception:
qWarning('Publisher._change_publisher_rate(): could not parse rate value: %s' % (new_value))
else:
publisher_info['rate'] = rate
#qDebug('Publisher._change_publisher_rate(): %s rate changed: %fHz' % (publisher_info['topic_name'], publisher_info['rate']))
publisher_info['timer'].stop()
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
# make sure the column value reflects the actual rate
return '%.2f' % publisher_info['rate']
def _change_publisher_expression(self, publisher_info, topic_name, new_value):
expression = str(new_value)
if len(expression) == 0:
if topic_name in publisher_info['expressions']:
del publisher_info['expressions'][topic_name]
#qDebug('Publisher._change_publisher_expression(): removed expression for: %s' % (topic_name))
else:
slot_type, is_array = get_field_type(topic_name)
if is_array:
slot_type = list
# strip possible trailing error message from expression
error_prefix = '# error'
error_prefix_pos = expression.find(error_prefix)
if error_prefix_pos >= 0:
expression = expression[:error_prefix_pos]
success, _ = self._evaluate_expression(expression, slot_type)
if success:
old_expression = publisher_info['expressions'].get(topic_name, None)
publisher_info['expressions'][topic_name] = expression
#print 'Publisher._change_publisher_expression(): topic: %s, type: %s, expression: %s' % (topic_name, slot_type, new_value)
self._fill_message_slots(publisher_info['message_instance'], publisher_info['topic_name'], publisher_info['expressions'], publisher_info['counter'])
try:
publisher_info['message_instance']._check_types()
except Exception, e:
error_str = str(e)
print 'serialization error:', error_str
if old_expression is not None:
publisher_info['expressions'][topic_name] = old_expression
else:
del publisher_info['expressions'][topic_name]
return '%s %s: %s' % (expression, error_prefix, error_str)
return expression
else:
class PerspectiveManager(QObject):
"""Manager for perspectives associated with specific sets of `Settings`."""
perspective_changed_signal = Signal(str)
save_settings_signal = Signal(Settings, Settings)
restore_settings_signal = Signal(Settings, Settings)
restore_settings_without_plugin_changes_signal = Signal(Settings, Settings)
HIDDEN_PREFIX = '@'
def __init__(self, settings, application_context):
super(PerspectiveManager, self).__init__()
self.setObjectName('PerspectiveManager')
self._qtgui_path = application_context.qtgui_path
self._settings_proxy = SettingsProxy(settings)
self._global_settings = Settings(self._settings_proxy, 'global')
self._perspective_settings = None
self._create_perspective_dialog = None
self._menu_manager = None
self._perspective_mapper = None
# get perspective list from settings
self.perspectives = self._settings_proxy.value('', 'perspectives', [])
if is_string(self.perspectives):
self.perspectives = [self.perspectives]
self._current_perspective = None
self._remove_action = None
self._callback = None
self._callback_args = []
if application_context.provide_app_dbus_interfaces:
from .perspective_manager_dbus_interface import PerspectiveManagerDBusInterface
self._dbus_server = PerspectiveManagerDBusInterface(
self, application_context)
def set_menu(self, menu):
self._menu_manager = MenuManager(menu)
self._perspective_mapper = QSignalMapper(menu)
self._perspective_mapper.mapped[str].connect(self.switch_perspective)
# generate menu
create_action = QAction('&Create perspective...',
self._menu_manager.menu)
create_action.setIcon(QIcon.fromTheme('list-add'))
create_action.triggered.connect(self._on_create_perspective)
self._menu_manager.add_suffix(create_action)
self._remove_action = QAction('&Remove perspective...',
self._menu_manager.menu)
self._remove_action.setEnabled(False)
self._remove_action.setIcon(QIcon.fromTheme('list-remove'))
self._remove_action.triggered.connect(self._on_remove_perspective)
self._menu_manager.add_suffix(self._remove_action)
self._menu_manager.add_suffix(None)
import_action = QAction('&Import...', self._menu_manager.menu)
import_action.setIcon(QIcon.fromTheme('document-open'))
import_action.triggered.connect(self._on_import_perspective)
self._menu_manager.add_suffix(import_action)
export_action = QAction('&Export...', self._menu_manager.menu)
export_action.setIcon(QIcon.fromTheme('document-save-as'))
export_action.triggered.connect(self._on_export_perspective)
self._menu_manager.add_suffix(export_action)
# add perspectives to menu
for name in self.perspectives:
if not name.startswith(self.HIDDEN_PREFIX):
self._add_perspective_action(name)
def set_perspective(self, name, hide_and_without_plugin_changes=False):
if name is None:
name = self._settings_proxy.value('', 'current-perspective',
'Default')
elif hide_and_without_plugin_changes:
name = self.HIDDEN_PREFIX + name
self.switch_perspective(
name,
save_before=not hide_and_without_plugin_changes,
without_plugin_changes=hide_and_without_plugin_changes)
@Slot(str)
@Slot(str, bool)
@Slot(str, bool, bool)
def switch_perspective(self,
name,
settings_changed=True,
save_before=True,
without_plugin_changes=False):
if save_before and self._global_settings is not None and self._perspective_settings is not None:
self._callback = self._switch_perspective
self._callback_args = [name, settings_changed, save_before]
self.save_settings_signal.emit(self._global_settings,
self._perspective_settings)
else:
self._switch_perspective(name, settings_changed, save_before,
without_plugin_changes)
def _switch_perspective(self,
name,
settings_changed,
save_before,
without_plugin_changes=False):
# convert from unicode
name = str(name.replace('/', '__'))
qDebug(
'PerspectiveManager.switch_perspective() switching to perspective "%s"'
% name)
if self._current_perspective is not None and self._menu_manager is not None:
self._menu_manager.set_item_checked(self._current_perspective,
False)
self._menu_manager.set_item_disabled(self._current_perspective,
False)
# create perspective if necessary
if name not in self.perspectives:
self._create_perspective(name, clone_perspective=False)
# update current perspective
self._current_perspective = name
if self._menu_manager is not None:
self._menu_manager.set_item_checked(self._current_perspective,
True)
self._menu_manager.set_item_disabled(self._current_perspective,
True)
if not self._current_perspective.startswith(self.HIDDEN_PREFIX):
self._settings_proxy.set_value('', 'current-perspective',
self._current_perspective)
self._perspective_settings = self._get_perspective_settings(
self._current_perspective)
# emit signals
self.perspective_changed_signal.emit(
self._current_perspective.lstrip(self.HIDDEN_PREFIX))
if settings_changed:
if not without_plugin_changes:
self.restore_settings_signal.emit(self._global_settings,
self._perspective_settings)
else:
self.restore_settings_without_plugin_changes_signal.emit(
self._global_settings, self._perspective_settings)
def save_settings_completed(self):
if self._callback is not None:
callback = self._callback
callback_args = self._callback_args
self._callback = None
self._callback_args = []
callback(*callback_args)
def _get_perspective_settings(self, perspective_name):
return Settings(self._settings_proxy,
'perspective/%s' % perspective_name)
def _on_create_perspective(self):
name = self._choose_new_perspective_name()
if name is not None:
clone_perspective = self._create_perspective_dialog.clone_checkbox.isChecked(
)
self._create_perspective(name, clone_perspective)
self.switch_perspective(name,
settings_changed=not clone_perspective,
save_before=False)
def _choose_new_perspective_name(self, show_cloning=True):
# input dialog for new perspective name
if self._create_perspective_dialog is None:
ui_file = os.path.join(self._qtgui_path, 'resource',
'perspective_create.ui')
self._create_perspective_dialog = loadUi(ui_file)
# custom validator preventing forward slashs
class CustomValidator(QValidator):
def __init__(self, parent=None):
super(CustomValidator, self).__init__(parent)
def fixup(self, value):
value = value.replace('/', '')
def validate(self, value, pos):
if value.find('/') != -1:
pos = value.find('/')
return (QValidator.Invalid, value, pos)
if value == '':
return (QValidator.Intermediate, value, pos)
return (QValidator.Acceptable, value, pos)
self._create_perspective_dialog.perspective_name_edit.setValidator(
CustomValidator())
# set default values
self._create_perspective_dialog.perspective_name_edit.setText('')
self._create_perspective_dialog.clone_checkbox.setChecked(True)
self._create_perspective_dialog.clone_checkbox.setVisible(show_cloning)
# show dialog and wait for it's return value
return_value = self._create_perspective_dialog.exec_()
if return_value == self._create_perspective_dialog.Rejected:
return
name = str(self._create_perspective_dialog.perspective_name_edit.text(
)).lstrip(self.HIDDEN_PREFIX)
if name == '':
QMessageBox.warning(
self._menu_manager.menu, self.tr('Empty perspective name'),
self.tr('The name of the perspective must be non-empty.'))
return
if name in self.perspectives:
QMessageBox.warning(
self._menu_manager.menu, self.tr('Duplicate perspective name'),
self.tr('A perspective with the same name already exists.'))
return
return name
def _create_perspective(self, name, clone_perspective=True):
# convert from unicode
name = str(name)
if name.find('/') != -1:
raise RuntimeError(
'PerspectiveManager._create_perspective() name must not contain forward slashs (/)'
)
qDebug('PerspectiveManager._create_perspective(%s, %s)' %
(name, clone_perspective))
# add to list of perspectives
self.perspectives.append(name)
self._settings_proxy.set_value('', 'perspectives', self.perspectives)
# save current settings
if self._global_settings is not None and self._perspective_settings is not None:
self._callback = self._create_perspective_continued
self._callback_args = [name, clone_perspective]
self.save_settings_signal.emit(self._global_settings,
self._perspective_settings)
else:
self._create_perspective_continued(name, clone_perspective)
def _create_perspective_continued(self, name, clone_perspective):
# clone settings
if clone_perspective:
new_settings = self._get_perspective_settings(name)
keys = self._perspective_settings.all_keys()
for key in keys:
value = self._perspective_settings.value(key)
new_settings.set_value(key, value)
# add and switch to perspective
if not name.startswith(self.HIDDEN_PREFIX):
self._add_perspective_action(name)
def _add_perspective_action(self, name):
if self._menu_manager is not None:
# create action
action = QAction(name, self._menu_manager.menu)
action.setCheckable(True)
self._perspective_mapper.setMapping(action, name)
action.triggered.connect(self._perspective_mapper.map)
# add action to menu
self._menu_manager.add_item(action)
# enable remove-action
if self._menu_manager.count_items() > 1:
self._remove_action.setEnabled(True)
def _on_remove_perspective(self):
# input dialog to choose perspective to be removed
names = list(self.perspectives)
names.remove(self._current_perspective)
name, return_value = QInputDialog.getItem(
self._menu_manager.menu,
self._menu_manager.tr('Remove perspective'),
self._menu_manager.tr('Select the perspective'), names, 0, False)
# convert from unicode
name = str(name)
if return_value == QInputDialog.Rejected:
return
self._remove_perspective(name)
def _remove_perspective(self, name):
if name not in self.perspectives:
raise UserWarning('unknown perspective: %s' % name)
qDebug('PerspectiveManager._remove_perspective(%s)' % str(name))
# remove from list of perspectives
self.perspectives.remove(name)
self._settings_proxy.set_value('', 'perspectives', self.perspectives)
# remove settings
settings = self._get_perspective_settings(name)
settings.remove('')
# remove from menu
self._menu_manager.remove_item(name)
# disable remove-action
if self._menu_manager.count_items() < 2:
self._remove_action.setEnabled(False)
def _on_import_perspective(self):
file_name, _ = QFileDialog.getOpenFileName(
self._menu_manager.menu, self.tr('Import perspective from file'),
None, self.tr('Perspectives (*.perspective)'))
if file_name is None or file_name == '':
return
perspective_name = os.path.basename(file_name)
suffix = '.perspective'
if perspective_name.endswith(suffix):
perspective_name = perspective_name[:-len(suffix)]
if perspective_name in self.perspectives:
perspective_name = self._choose_new_perspective_name(False)
if perspective_name is None:
return
self.import_perspective_from_file(file_name, perspective_name)
def import_perspective_from_file(self, path, perspective_name):
# create clean perspective
if perspective_name in self.perspectives:
self._remove_perspective(perspective_name)
self._create_perspective(perspective_name, clone_perspective=False)
# read perspective from file
file_handle = open(path, 'r')
#data = eval(file_handle.read())
data = json.loads(file_handle.read())
self._convert_values(data, self._import_value)
new_settings = self._get_perspective_settings(perspective_name)
self._set_dict_on_settings(data, new_settings)
self.switch_perspective(perspective_name,
settings_changed=True,
save_before=True)
def _set_dict_on_settings(self, data, settings):
"""Set dictionary key-value pairs on Settings instance."""
keys = data.get('keys', {})
for key in keys:
settings.set_value(key, keys[key])
groups = data.get('groups', {})
for group in groups:
sub = settings.get_settings(group)
self._set_dict_on_settings(groups[group], sub)
def _on_export_perspective(self):
file_name, _ = QFileDialog.getSaveFileName(
self._menu_manager.menu, self.tr('Export perspective to file'),
self._current_perspective + '.perspective',
self.tr('Perspectives (*.perspective)'))
if file_name is None or file_name == '':
return
# trigger save of perspective before export
self._callback = self._on_export_perspective_continued
self._callback_args = [file_name]
self.save_settings_signal.emit(self._global_settings,
self._perspective_settings)
def _on_export_perspective_continued(self, file_name):
# convert every value
data = self._get_dict_from_settings(self._perspective_settings)
self._convert_values(data, self._export_value)
# write perspective data to file
file_handle = open(file_name, 'w')
file_handle.write(json.dumps(data, indent=2, separators=(',', ': ')))
file_handle.close()
def _get_dict_from_settings(self, settings):
"""Convert data of Settings instance to dictionary."""
keys = {}
for key in settings.child_keys():
keys[str(key)] = settings.value(key)
groups = {}
for group in settings.child_groups():
sub = settings.get_settings(group)
groups[str(group)] = self._get_dict_from_settings(sub)
return {'keys': keys, 'groups': groups}
def _convert_values(self, data, convert_function):
keys = data.get('keys', {})
for key in keys:
keys[key] = convert_function(keys[key])
groups = data.get('groups', {})
for group in groups:
self._convert_values(groups[group], convert_function)
def _import_value(self, value):
import QtCore # @UnusedImport
if value['type'] == 'repr':
return eval(value['repr'])
elif value['type'] == 'repr(QByteArray.hex)':
return QByteArray.fromHex(eval(value['repr(QByteArray.hex)']))
raise RuntimeError(
'PerspectiveManager._import_value() unknown serialization type (%s)'
% value['type'])
def _export_value(self, value):
data = {}
if value.__class__.__name__ == 'QByteArray':
hex_value = value.toHex()
data['repr(QByteArray.hex)'] = self._strip_qt_binding_prefix(
hex_value, repr(hex_value))
data['type'] = 'repr(QByteArray.hex)'
# add pretty print for better readability
characters = ''
for i in range(1, value.size(), 2):
try:
character = value.at(i)
# output all non-control characters
if character >= ' ' and character <= '~':
characters += character
else:
characters += ' '
except UnicodeDecodeError:
characters += ' '
data['pretty-print'] = characters
else:
data['repr'] = self._strip_qt_binding_prefix(value, repr(value))
data['type'] = 'repr'
# verify that serialized data can be deserialized correctly
reimported = self._import_value(data)
if reimported != value:
raise RuntimeError(
'PerspectiveManager._export_value() stored value can not be restored (%s)'
% type(value))
return data
def _strip_qt_binding_prefix(self, obj, data):
"""Strip binding specific prefix from type string."""
parts = obj.__class__.__module__.split('.')
if len(parts) > 1 and parts[1] == 'QtCore':
prefix = '.'.join(parts[:2])
data = data.replace(prefix, 'QtCore', 1)
return data
def __init__(self):
super(ROS_Publisher, self).__init__()
self.id_counter = 0
self.publisher_dict = {}
self._timeout_mapper = QSignalMapper(self)
self._timeout_mapper.mapped[int].connect(self.publish_once)
class Publisher(Plugin):
def __init__(self, context):
super(Publisher, self).__init__(context)
self.setObjectName('Publisher')
self._node = context.node
# create widget
self._widget = PublisherWidget(self._node)
self._widget.add_publisher.connect(self.add_publisher)
self._widget.change_publisher.connect(self.change_publisher)
self._widget.publish_once.connect(self.publish_once)
self._widget.remove_publisher.connect(self.remove_publisher)
self._widget.clean_up_publishers.connect(self.clean_up_publishers)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# create context for the expression eval statement
self._eval_locals = {'i': 0}
for module in (math, random, time):
self._eval_locals.update(module.__dict__)
del self._eval_locals['__name__']
del self._eval_locals['__doc__']
self._publishers = {}
self._id_counter = 0
self._timeout_mapper = QSignalMapper(self)
self._timeout_mapper.mapped[int].connect(self.publish_once)
# add our self to the main window
context.add_widget(self._widget)
@Slot(str, str, float, bool)
def add_publisher(self, topic_name, type_name, rate, enabled):
topic_name = str(topic_name)
try:
self._node._validate_topic_or_service_name(topic_name)
except InvalidTopicNameException as e:
qWarning(str(e))
return
publisher_info = {
'topic_name': topic_name,
'type_name': str(type_name),
'rate': float(rate),
'enabled': bool(enabled),
}
self._add_publisher(publisher_info)
def _add_publisher(self, publisher_info):
publisher_info['publisher_id'] = self._id_counter
self._id_counter += 1
publisher_info['counter'] = 0
publisher_info['enabled'] = publisher_info.get('enabled', False)
publisher_info['expressions'] = publisher_info.get('expressions', {})
publisher_info['message_instance'] = self._create_message_instance(
publisher_info['type_name'])
if publisher_info['message_instance'] is None:
return
msg_module = get_message_class(publisher_info['type_name'])
if not msg_module:
raise RuntimeError(
'The passed message type "{}" is invalid'.format(
publisher_info['type_name']))
# Topic name provided was relative, remap to node namespace (if it was set)
if not publisher_info['topic_name'].startswith('/'):
publisher_info['topic_name'] = \
self._node.get_namespace() + publisher_info['topic_name']
# create publisher and timer
publisher_info['publisher'] = self._node.create_publisher(
msg_module,
publisher_info['topic_name'],
qos_profile=QoSProfile(depth=10))
publisher_info['timer'] = QTimer(self)
# add publisher info to _publishers dict and create signal mapping
self._publishers[publisher_info['publisher_id']] = publisher_info
self._timeout_mapper.setMapping(publisher_info['timer'],
publisher_info['publisher_id'])
publisher_info['timer'].timeout.connect(self._timeout_mapper.map)
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
self._widget.publisher_tree_widget.model().add_publisher(
publisher_info)
@Slot(int, str, str, str, object)
def change_publisher(self, publisher_id, topic_name, column_name,
new_value, setter_callback):
handler = getattr(self, '_change_publisher_%s' % column_name, None)
if handler is not None:
new_text = handler(self._publishers[publisher_id], topic_name,
new_value)
if new_text is not None:
setter_callback(new_text)
def _change_publisher_topic(self, publisher_info, topic_name, new_value):
publisher_info['enabled'] = (new_value and new_value.lower()
in ['1', 'true', 'yes'])
# qDebug(
# 'Publisher._change_publisher_enabled(): %s enabled: %s' %
# (publisher_info['topic_name'], publisher_info['enabled']))
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
else:
publisher_info['timer'].stop()
return None
def _change_publisher_type(self, publisher_info, topic_name, new_value):
type_name = new_value
# create new slot
slot_value = self._create_message_instance(type_name)
# find parent slot
slot_path = topic_name[len(publisher_info['topic_name']):].strip(
'/').split('/')
parent_slot = eval('.'.join(["publisher_info['message_instance']"] +
slot_path[:-1]))
# find old slot
slot_name = slot_path[-1]
slot_index = parent_slot.__slots__.index(slot_name)
# restore type if user value was invalid
if slot_value is None:
qWarning(
'Publisher._change_publisher_type(): could not find type: %s' %
(type_name))
return parent_slot._slot_types[slot_index]
else:
# replace old slot
parent_slot._slot_types[slot_index] = type_name
setattr(parent_slot, slot_name, slot_value)
self._widget.publisher_tree_widget.model().update_publisher(
publisher_info)
def _change_publisher_rate(self, publisher_info, topic_name, new_value):
try:
rate = float(new_value)
except Exception:
qWarning(
'Publisher._change_publisher_rate(): could not parse rate value: %s'
% (new_value))
else:
publisher_info['rate'] = rate
# qDebug(
# 'Publisher._change_publisher_rate(): %s rate changed: %fHz' %
# (publisher_info['topic_name'], publisher_info['rate']))
publisher_info['timer'].stop()
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(
int(1000.0 / publisher_info['rate']))
# make sure the column value reflects the actual rate
return '%.2f' % publisher_info['rate']
def _change_publisher_expression(self, publisher_info, topic_name,
new_value):
expression = str(new_value)
if len(expression) == 0:
if topic_name in publisher_info['expressions']:
del publisher_info['expressions'][topic_name]
# qDebug(
# 'Publisher._change_publisher_expression(): removed expression'
# 'for: %s' % (topic_name))
else:
# Strip topic name from the full topic path
slot_path = topic_name.replace(publisher_info['topic_name'], '', 1)
slot_path, slot_array_index = self._extract_array_info(slot_path)
# Get the property type from the message class
slot_type, is_array = \
get_slot_type(publisher_info['message_instance'].__class__, slot_path)
if slot_array_index is not None:
is_array = False
if is_array:
slot_type = list
# strip possible trailing error message from expression
error_prefix = '# error'
error_prefix_pos = expression.find(error_prefix)
if error_prefix_pos >= 0:
expression = expression[:error_prefix_pos]
success, _ = self._evaluate_expression(expression, slot_type)
if success:
old_expression = publisher_info['expressions'].get(
topic_name, None)
publisher_info['expressions'][topic_name] = expression
try:
self._fill_message_slots(
publisher_info['message_instance'],
publisher_info['topic_name'],
publisher_info['expressions'],
publisher_info['counter'])
except Exception as e:
if old_expression is not None:
publisher_info['expressions'][
topic_name] = old_expression
else:
del publisher_info['expressions'][topic_name]
return '%s %s: %s' % (expression, error_prefix, e)
return expression
else:
return '%s %s evaluating as "%s"' % (expression, error_prefix,
slot_type.__name__)
def _extract_array_info(self, type_str):
array_size = None
if '[' in type_str and type_str[-1] == ']':
type_str, array_size_str = type_str.split('[', 1)
array_size_str = array_size_str[:-1]
if len(array_size_str) > 0:
array_size = int(array_size_str)
else:
array_size = 0
return type_str, array_size
def _create_message_instance(self, type_str):
base_type_str, array_size = self._extract_array_info(type_str)
try:
base_message_type = get_message_class(base_type_str)
except LookupError as e:
qWarning(
"Creating message type {} failed. Please check your spelling and that the "
"message package has been built\n{}".format(base_type_str, e))
return None
if base_message_type is None:
return None
if array_size is not None:
message = []
for _ in range(array_size):
message.append(base_message_type())
else:
message = base_message_type()
return message
def _evaluate_expression(self, expression, slot_type):
global _list_types
global _numeric_types
successful_eval = True
try:
# try to evaluate expression
value = eval(expression, {}, self._eval_locals)
except Exception as e:
qWarning(
'Python eval failed for expression "{}"'.format(expression) +
' with an exception "{}"'.format(e))
successful_eval = False
if slot_type is str:
if successful_eval:
value = str(value)
else:
# for string slots just convert the expression to str, if it did not
# evaluate successfully
value = str(expression)
successful_eval = True
elif successful_eval:
type_set = set((slot_type, type(value)))
# check if value's type and slot_type belong to the same type group, i.e. array types,
# numeric types and if they do, make sure values's type is converted to the exact
# slot_type
if type_set <= set(_list_types) or type_set <= set(_numeric_types):
# convert to the right type
value = slot_type(value)
if successful_eval and isinstance(value, slot_type):
return True, value
else:
qWarning('Publisher._evaluate_expression(): failed to evaluate ' +
'expression: "%s" as Python type "%s"' %
(expression, slot_type))
return False, None
def _fill_message_slots(self, message, topic_name, expressions, counter):
global _list_types
if topic_name in expressions and len(expressions[topic_name]) > 0:
# get type
if hasattr(message, '_type'):
message_type = message._type
else:
message_type = type(message)
self._eval_locals['i'] = counter
success, value = self._evaluate_expression(expressions[topic_name],
message_type)
if not success:
value = message_type()
return value
# if no expression exists for this topic_name, continue with it's child slots
elif hasattr(message, 'get_fields_and_field_types'):
for slot_name in message.get_fields_and_field_types().keys():
value = self._fill_message_slots(getattr(message, slot_name),
topic_name + '/' + slot_name,
expressions, counter)
if value is not None:
setattr(message, slot_name, value)
elif type(message) in _list_types and (len(message) > 0):
for index, slot in enumerate(message):
value = self._fill_message_slots(slot,
topic_name + '[%d]' % index,
expressions, counter)
# this deals with primitive-type arrays
if not hasattr(message[0], '__slots__') and value is not None:
message[index] = value
return None
@Slot(int)
def publish_once(self, publisher_id):
publisher_info = self._publishers.get(publisher_id, None)
if publisher_info is not None:
publisher_info['counter'] += 1
self._fill_message_slots(publisher_info['message_instance'],
publisher_info['topic_name'],
publisher_info['expressions'],
publisher_info['counter'])
publisher_info['publisher'].publish(
publisher_info['message_instance'])
@Slot(int)
def remove_publisher(self, publisher_id):
publisher_info = self._publishers.get(publisher_id, None)
if publisher_info is not None:
publisher_info['timer'].stop()
self._node.destroy_publisher(publisher_info['publisher'])
del publisher_info['publisher']
del self._publishers[publisher_id]
def save_settings(self, plugin_settings, instance_settings):
publisher_copies = []
for publisher in self._publishers.values():
publisher_copy = {}
publisher_copy.update(publisher)
publisher_copy['enabled'] = False
del publisher_copy['timer']
del publisher_copy['message_instance']
del publisher_copy['publisher']
publisher_copies.append(publisher_copy)
instance_settings.set_value('publishers', repr(publisher_copies))
def restore_settings(self, plugin_settings, instance_settings):
# If changing perspectives and rqt_publisher is already loaded, we need to clean up the
# previously existing publishers
self.clean_up_publishers()
publishers = eval(instance_settings.value('publishers', '[]'))
for publisher in publishers:
self._add_publisher(publisher)
def clean_up_publishers(self):
self._widget.publisher_tree_widget.model().clear()
for publisher_info in self._publishers.values():
publisher_info['timer'].stop()
self._node.destroy_publisher(publisher_info['publisher'])
self._publishers = {}
def shutdown_plugin(self):
self._widget.shutdown_plugin()
self.clean_up_publishers()
class Publisher(Plugin):
def __init__(self, context):
super(Publisher, self).__init__(context)
self.setObjectName('Publisher')
# create widget
self._widget = PublisherWidget()
self._widget.add_publisher.connect(self.add_publisher)
self._widget.change_publisher.connect(self.change_publisher)
self._widget.publish_once.connect(self.publish_once)
self._widget.remove_publisher.connect(self.remove_publisher)
self._widget.clean_up_publishers.connect(self.clean_up_publishers)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# create context for the expression eval statement
self._eval_locals = {'i': 0}
for module in (math, random, time):
self._eval_locals.update(module.__dict__)
self._eval_locals['genpy'] = genpy
del self._eval_locals['__name__']
del self._eval_locals['__doc__']
self._publishers = {}
self._id_counter = 0
self._timeout_mapper = QSignalMapper(self)
self._timeout_mapper.mapped[int].connect(self.publish_once)
# add our self to the main window
context.add_widget(self._widget)
@Slot(str, str, float, bool)
def add_publisher(self, topic_name, type_name, rate, enabled):
publisher_info = {
'topic_name': str(topic_name),
'type_name': str(type_name),
'rate': float(rate),
'enabled': bool(enabled),
}
self._add_publisher(publisher_info)
def _add_publisher(self, publisher_info):
publisher_info['publisher_id'] = self._id_counter
self._id_counter += 1
publisher_info['counter'] = 0
publisher_info['enabled'] = publisher_info.get('enabled', False)
publisher_info['expressions'] = publisher_info.get('expressions', {})
publisher_info['message_instance'] = self._create_message_instance(
publisher_info['type_name'])
if publisher_info['message_instance'] is None:
return
# create publisher and timer
try:
publisher_info['publisher'] = rospy.Publisher(
publisher_info['topic_name'],
type(publisher_info['message_instance']),
queue_size=100)
except TypeError:
publisher_info['publisher'] = rospy.Publisher(
publisher_info['topic_name'],
type(publisher_info['message_instance']))
publisher_info['timer'] = QTimer(self)
# add publisher info to _publishers dict and create signal mapping
self._publishers[publisher_info['publisher_id']] = publisher_info
self._timeout_mapper.setMapping(publisher_info['timer'],
publisher_info['publisher_id'])
publisher_info['timer'].timeout.connect(self._timeout_mapper.map)
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
self._widget.publisher_tree_widget.model().add_publisher(
publisher_info)
@Slot(int, str, str, str, object)
def change_publisher(self, publisher_id, topic_name, column_name,
new_value, setter_callback):
handler = getattr(self, '_change_publisher_%s' % column_name, None)
if handler is not None:
new_text = handler(self._publishers[publisher_id], topic_name,
new_value)
if new_text is not None:
setter_callback(new_text)
def _change_publisher_topic(self, publisher_info, topic_name, new_value):
publisher_info['enabled'] = (new_value and new_value.lower()
in ['1', 'true', 'yes'])
#qDebug('Publisher._change_publisher_enabled(): %s enabled: %s' % (publisher_info['topic_name'], publisher_info['enabled']))
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
else:
publisher_info['timer'].stop()
return None
def _change_publisher_type(self, publisher_info, topic_name, new_value):
type_name = new_value
# create new slot
slot_value = self._create_message_instance(type_name)
# find parent slot
slot_path = topic_name[len(publisher_info['topic_name']):].strip(
'/').split('/')
parent_slot = eval('.'.join(["publisher_info['message_instance']"] +
slot_path[:-1]))
# find old slot
slot_name = slot_path[-1]
slot_index = parent_slot.__slots__.index(slot_name)
# restore type if user value was invalid
if slot_value is None:
qWarning(
'Publisher._change_publisher_type(): could not find type: %s' %
(type_name))
return parent_slot._slot_types[slot_index]
else:
# replace old slot
parent_slot._slot_types[slot_index] = type_name
setattr(parent_slot, slot_name, slot_value)
self._widget.publisher_tree_widget.model().update_publisher(
publisher_info)
def _change_publisher_rate(self, publisher_info, topic_name, new_value):
try:
rate = float(new_value)
except Exception:
qWarning(
'Publisher._change_publisher_rate(): could not parse rate value: %s'
% (new_value))
else:
publisher_info['rate'] = rate
#qDebug('Publisher._change_publisher_rate(): %s rate changed: %fHz' % (publisher_info['topic_name'], publisher_info['rate']))
publisher_info['timer'].stop()
if publisher_info['enabled'] and publisher_info['rate'] > 0:
publisher_info['timer'].start(
int(1000.0 / publisher_info['rate']))
# make sure the column value reflects the actual rate
return '%.2f' % publisher_info['rate']
def _change_publisher_expression(self, publisher_info, topic_name,
new_value):
expression = str(new_value)
if len(expression) == 0:
if topic_name in publisher_info['expressions']:
del publisher_info['expressions'][topic_name]
#qDebug('Publisher._change_publisher_expression(): removed expression for: %s' % (topic_name))
else:
slot_type, is_array = get_field_type(topic_name)
if is_array:
slot_type = list
# strip possible trailing error message from expression
error_prefix = '# error'
error_prefix_pos = expression.find(error_prefix)
if error_prefix_pos >= 0:
expression = expression[:error_prefix_pos]
success, _ = self._evaluate_expression(expression, slot_type)
if success:
old_expression = publisher_info['expressions'].get(
topic_name, None)
publisher_info['expressions'][topic_name] = expression
#print 'Publisher._change_publisher_expression(): topic: %s, type: %s, expression: %s' % (topic_name, slot_type, new_value)
self._fill_message_slots(publisher_info['message_instance'],
publisher_info['topic_name'],
publisher_info['expressions'],
publisher_info['counter'])
try:
publisher_info['message_instance']._check_types()
except Exception, e:
error_str = str(e)
print 'serialization error:', error_str
if old_expression is not None:
publisher_info['expressions'][
topic_name] = old_expression
else:
del publisher_info['expressions'][topic_name]
return '%s %s: %s' % (expression, error_prefix, error_str)
return expression
else:
def __init__(self, context, add_execute_widget=True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left",
PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left",
PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up",
PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left",
PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward",
PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward",
PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over",
PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet",
PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance",
PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right",
PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right",
PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down",
PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right",
PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit,
"Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number,
"Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index,
"Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox,
self.start_foot_selection_combo_box,
"Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.0, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance,
"Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step,
"Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(
0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation,
"Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(
self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz,
"delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger=self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(
vbox,
QExecuteStepPlanWidget(logger=self.logger,
step_plan_topic="step_plan"),
"Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(
self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(
self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet,
self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan",
StepPlan,
queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient(
"step_plan_request", StepPlanRequestAction)
class PluginManagerWidget(QObject):
_NUM_DESC_ATTRIBUTES = 5
plugin_states_updated_signal = Signal(list)
def __init__(self, context):
super(PluginManagerWidget, self).__init__()
self.namespace = '/'
self.plugin_states_update_sub = None
self.load_plugin_set_client = None
self.get_plugin_descriptions_client = None
self.get_plugin_states_client = None
self.add_plugin_client = None
self.remove_plugin_client = None
self.plugin_descriptions = []
self.add_plugin_selection_filter = PluginDescription()
# start widget
widget = context
vbox = QVBoxLayout()
# load from ui
self.plugin_manager_widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('vigir_pluginlib_manager'),
'resource', 'plugin_manager.ui')
loadUi(ui_file, self.plugin_manager_widget, {'QWidget': QWidget})
vbox.addWidget(self.plugin_manager_widget)
# init ui
icon = QIcon.fromTheme("view-refresh")
self.plugin_manager_widget.refreshAllPushButton.setIcon(icon)
self.plugin_manager_widget.refreshPluginStatesPushButton.setIcon(icon)
# init tree view
tree_view = self.plugin_manager_widget.plugin_tree_view
tree_view.setSelectionMode(QAbstractItemView.ExtendedSelection)
tree_view.setContextMenuPolicy(Qt.CustomContextMenu)
tree_view.customContextMenuRequested.connect(self._open_context_menu)
self.plugin_tree_model = PluginTreeModel()
tree_view.setModel(self.plugin_tree_model)
# set up combo boxes
self.plugin_manager_widget.pluginNameComboBox.setInsertPolicy(
QComboBox.NoInsert)
# references to combo boxes
self.plugin_cb = []
self.plugin_cb.append(self.plugin_manager_widget.pluginNameComboBox)
self.plugin_cb.append(
self.plugin_manager_widget.pluginTypeClassComboBox)
self.plugin_cb.append(
self.plugin_manager_widget.pluginTypePackageComboBox)
self.plugin_cb.append(
self.plugin_manager_widget.pluginBaseClassComboBox)
self.plugin_cb.append(
self.plugin_manager_widget.pluginBasePackageComboBox)
# init signal mapper
self.plugin_cb_mapper = QSignalMapper(self)
self.plugin_cb_mapper.mapped.connect(self.add_plugin_selection_changed)
# connect to signals
for i in range(len(self.plugin_cb)):
self.plugin_cb_mapper.setMapping(self.plugin_cb[i], i)
self.plugin_cb[i].currentIndexChanged.connect(
self.plugin_cb_mapper.map)
self.plugin_manager_widget.namespaceComboBox.currentIndexChanged[
str].connect(self.set_namespace)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(
self.search_namespace)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(
self.refresh_plugin_descriptions)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(
self.refresh_plugin_states)
self.plugin_manager_widget.loadPluginSetPushButton.clicked[
bool].connect(self.load_plugin_set)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[
bool].connect(self.refresh_plugin_sets)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[
bool].connect(self.refresh_plugin_descriptions)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[
bool].connect(self.refresh_plugin_states)
self.plugin_manager_widget.addPluginPushButton.clicked[bool].connect(
self.add_plugin)
self.plugin_manager_widget.clearAddPluginSelectionPushButton.clicked[
bool].connect(self.clear_add_plugin_selection)
self.plugin_manager_widget.removePluginsPushButton.clicked[
bool].connect(self.remove_plugins)
# Qt signals
self.plugin_states_updated_signal.connect(self.update_plugin_tree_view)
#self.connect(self, QtCore.SIGNAL('setTransitionModeStatusStyle(PyQt_PyObject)'), self._set_transition_mode_status_style)
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init plugin tree view
self.search_namespace()
def shutdown_plugin(self):
print 'Shutting down ...'
self.plugin_states_update_sub.unregister()
print 'Done!'
def _open_context_menu(self, position):
indexes = self.plugin_manager_widget.plugin_tree_view.selectedIndexes()
level = -1
if len(indexes) > 0:
level = 0
index = indexes[0]
while index.parent().isValid():
index = index.parent()
level += 1
menu = QMenu()
if level == 0:
expand_action = QAction(self.tr('Expand'), None)
expand_action.triggered.connect(
self.plugin_manager_widget.plugin_tree_view.expandAll)
menu.addAction(expand_action)
if level == 0 or level == 1:
remove_action = QAction(self.tr('Remove'), None)
remove_action.triggered.connect(self.remove_plugins)
menu.addAction(remove_action)
menu.exec_(
self.plugin_manager_widget.plugin_tree_view.viewport().mapToGlobal(
position))
def init_topics(self, namespace):
# init subscribers
self.plugin_states_update_sub = rospy.Subscriber(
namespace + 'plugin_manager/plugin_states_update', PluginStates,
self.plugin_states_update)
# init action clients
self.load_plugin_set_client = actionlib.SimpleActionClient(
namespace + 'plugin_manager/load_plugin_set',
PluginManagementAction)
self.get_plugin_descriptions_client = actionlib.SimpleActionClient(
namespace + 'plugin_manager/get_plugin_descriptions',
GetPluginDescriptionsAction)
self.get_plugin_states_client = actionlib.SimpleActionClient(
namespace + 'plugin_manager/get_plugin_states',
GetPluginStatesAction)
self.add_plugin_client = actionlib.SimpleActionClient(
namespace + 'plugin_manager/add_plugin', PluginManagementAction)
self.remove_plugin_client = actionlib.SimpleActionClient(
namespace + 'plugin_manager/remove_plugin', PluginManagementAction)
print("Switched to namespace '" + namespace + "'")
def _set_data_in_description(self, description, index, data):
if index == 0:
description.name.data = data
if index == 1:
description.type_class.data = data
if index == 2:
description.type_class_package.data = data
if index == 3:
description.base_class.data = data
if index == 4:
description.base_class_package.data = data
return description
def _get_data_from_description(self, description, index):
if index == 0:
return description.name.data
if index == 1:
return description.type_class.data
if index == 2:
return description.type_class_package.data
if index == 3:
return description.base_class.data
if index == 4:
return description.base_class_package.data
def filter_descriptions(self, filtered_list, description_filter):
result = filtered_list
for i in range(self._NUM_DESC_ATTRIBUTES):
if not self._get_data_from_description(description_filter, i):
continue
result = filter(
lambda d: self._get_data_from_description(
description_filter, i) == self._get_data_from_description(
d, i), result)
return result
@Slot()
def search_namespace(self):
cb = self.plugin_manager_widget.namespaceComboBox
cb.blockSignals(True)
cb.setEnabled(False)
cb.clear()
cb.addItem('Updating...')
# get topic list
_, _, topic_type = rospy.get_master().getTopicTypes()
topic_dict = dict(topic_type)
# filter list
topic_dict_filtered = dict()
for k, v in topic_dict.items():
if v == 'vigir_pluginlib_msgs/GetPluginStatesActionGoal':
topic_dict_filtered[k] = v
# update combo box with found namespaces
cb.clear()
namespaces = [ns[:-37] for ns in sorted(topic_dict_filtered.keys())]
cb.addItems(namespaces)
if cb.count() > 0:
self.set_namespace(cb.currentText())
cb.setEnabled(True)
cb.blockSignals(False)
else:
cb.addItem('No topics available!')
@Slot(str)
def set_namespace(self, namespace):
self.namespace = namespace
self.init_topics(namespace)
self.refresh_plugin_sets()
self.refresh_plugin_descriptions()
self.refresh_plugin_states()
@Slot()
def refresh_plugin_sets(self):
plugin_sets = []
if rospy.has_param(self.namespace + '/plugin_sets'):
plugin_sets = rospy.get_param(self.namespace +
'/plugin_sets').keys()
cb = self.plugin_manager_widget.loadPluginSetComboBox
cb.clear()
if plugin_sets:
cb.addItems(plugin_sets)
cb.setEnabled(True)
self.plugin_manager_widget.loadPluginSetPushButton.setEnabled(True)
else:
cb.setEnabled(False)
self.plugin_manager_widget.loadPluginSetPushButton.setEnabled(
False)
@Slot()
def load_plugin_set(self):
if self.load_plugin_set_client.wait_for_server(rospy.Duration(1.0)):
# send request to server
goal = PluginManagementGoal()
goal.name.data = self.plugin_manager_widget.loadPluginSetComboBox.currentText(
)
self.load_plugin_set_client.send_goal(goal)
@Slot(int)
def add_plugin_selection_changed(self, index):
# update filter mask
self._set_data_in_description(self.add_plugin_selection_filter, index,
self.plugin_cb[index].currentText())
# block signals of combo boxes
for cb in self.plugin_cb:
cb.blockSignals(True)
# filter elements in combo boxes
filtered_descriptions = self.filter_descriptions(
self.plugin_descriptions, self.add_plugin_selection_filter)
for cb_index in range(self._NUM_DESC_ATTRIBUTES):
if cb_index != index:
rows_enabled = 0
last_enabled_row_index = 0
data = [
self._get_data_from_description(d, cb_index)
for d in filtered_descriptions
]
item_texts = [
self.plugin_cb[cb_index].itemText(i)
for i in range(self.plugin_cb[cb_index].count())
]
for row in range(1, len(item_texts)):
if not item_texts[row] or item_texts[row] in data:
self.plugin_cb[cb_index].setItemData(
row, 33, Qt.UserRole - 1) # enable item
rows_enabled += 1
last_enabled_row_index = row
else:
self.plugin_cb[cb_index].setItemData(
row, 0, Qt.UserRole - 1) # disable item
# if only one element is left, then auto select it
if rows_enabled == 1:
self.plugin_cb[cb_index].setCurrentIndex(
last_enabled_row_index)
# unblock signals of combo boxes
for cb in self.plugin_cb:
cb.blockSignals(False)
self.plugin_manager_widget.addPluginPushButton.setEnabled(True)
@Slot()
def clear_add_plugin_selection(self):
# block signals of combo boxes
for cb in self.plugin_cb:
cb.blockSignals(True)
self.plugin_cb[0].clearEditText()
for cb in self.plugin_cb:
cb.setCurrentIndex(0)
# reset selection filter
self.add_plugin_selection_filter = PluginDescription()
for cb in self.plugin_cb:
for row in range(cb.count()):
cb.setItemData(row, 33, Qt.UserRole - 1) # enable item
# unblock signals of combo boxes
for cb in self.plugin_cb:
cb.blockSignals(False)
self.plugin_manager_widget.addPluginPushButton.setEnabled(False)
@Slot()
def refresh_plugin_descriptions(self):
# clear old status
self.plugin_descriptions = []
for cb in self.plugin_cb:
cb.blockSignals(True)
cb.clear()
self.plugin_manager_widget.addPluginPushButton.setEnabled(False)
# collect all plugin descriptions from manager
if self.get_plugin_descriptions_client.wait_for_server(
rospy.Duration(1.0)):
self.get_plugin_descriptions_client.send_goal(
GetPluginDescriptionsGoal())
if self.get_plugin_descriptions_client.wait_for_result(
rospy.Duration(5.0)):
self.plugin_descriptions = self.get_plugin_descriptions_client.get_result(
).descriptions
# collect all plugins loaded into param server
all_params = rosparam.list_params(self.namespace)
for pname in all_params:
# remove the plugin manager namespace
if self.namespace == '/':
pname_sub = pname
else:
pname_sub = pname[len(self.namespace):]
psplit = pname_sub.split('/')
# get plugin description from param server
if len(psplit) >= 2 and psplit[1] == 'type_class':
description = PluginDescription()
description.name.data = psplit[0]
description.type_class.data = rospy.get_param(pname)
if rospy.has_param(self.namespace + psplit[0] +
'/type_class_package'):
description.type_class_package.data = rospy.get_param(
self.namespace + psplit[0] + '/type_class_package')
if rospy.has_param(self.namespace + psplit[0] + '/base_class'):
description.base_class.data = rospy.get_param(
self.namespace + psplit[0] + '/base_class')
if rospy.has_param(self.namespace + psplit[0] +
'/base_class_package'):
description.base_class_package.data = rospy.get_param(
self.namespace + psplit[0] + '/base_class_package')
self.plugin_descriptions.append(description)
# prepare combo box item texts
description = [[''] for i in range(self._NUM_DESC_ATTRIBUTES)]
for pd in self.plugin_descriptions:
for i in range(self._NUM_DESC_ATTRIBUTES):
description[i].append(self._get_data_from_description(pd, i))
# update combo boxes
for i in range(len(self.plugin_cb)):
description[i] = sorted(list(set(description[i])))
self.plugin_cb[i].addItems(description[i])
for cb in self.plugin_cb:
cb.blockSignals(False)
@Slot()
def refresh_plugin_states(self):
if self.get_plugin_states_client.wait_for_server(rospy.Duration(1.0)):
self.get_plugin_states_client.send_goal(GetPluginStatesGoal())
if self.get_plugin_states_client.wait_for_result(
rospy.Duration(5.0)):
self.plugin_states_updated_signal.emit(
self.get_plugin_states_client.get_result().states)
@Slot(PluginStates)
def plugin_states_update(self, plugin_states_msg):
self.plugin_states_updated_signal.emit(plugin_states_msg.states)
@Slot(list)
def update_plugin_tree_view(self, states):
self.plugin_tree_model.updateData(states)
#self.plugin_manager_widget.plugin_tree_view.setModel(self.plugin_tree_model)
#for column in range(0, self.plugin_tree_model.columnCount()):
# self.plugin_tree_model.resizeColumnToContents(column)
#self.plugin_manager_widget.plugin_tree_view.expandAll()
@Slot()
def add_plugin(self):
if self.add_plugin_client.wait_for_server(rospy.Duration(1.0)):
# generate plugin description
description = PluginDescription()
for i in range(self._NUM_DESC_ATTRIBUTES):
self._set_data_in_description(description, i,
self.plugin_cb[i].currentText())
# send request to server
goal = PluginManagementGoal()
goal.descriptions.append(description)
self.add_plugin_client.send_goal(goal)
@Slot()
def remove_plugins(self):
indexes = self.plugin_manager_widget.plugin_tree_view.selectionModel(
).selectedIndexes()
indexes = filter(lambda index: index.column() == 0, indexes)
# extract plugin descriptions from selection
descriptions = []
for index in indexes:
descriptions.append(
index.internalPointer().getPluginState().description)
# send request to server
if self.remove_plugin_client.wait_for_server(rospy.Duration(1.0)):
goal = PluginManagementGoal()
goal.descriptions = descriptions
self.remove_plugin_client.send_goal(goal)
class PluginHandlerXEmbedContainer(PluginHandler):
"""
Server part of the `PluginHandlerXEmbed`.
It starts the plugin in a subprocess and provides the `PluginHandlerDBusService` through a peer-to-peer DBus connection.
"""
_serial_number = 0
def __init__(self, parent, main_window, instance_id, application_context, container_manager, argv, dbus_object_path):
super(PluginHandlerXEmbedContainer, self).__init__(parent, main_window, instance_id, application_context, container_manager, argv)
self.setObjectName('PluginHandlerXEmbedContainer')
self._dbus_object_path = dbus_object_path
self._dbus_server = None
self._dbus_container_service = None
self._dbus_plugin_settings_service = None
self._dbus_instance_settings_service = None
self._process = None
self._pid = None
# mapping of widget object name to their embed container
self._embed_containers = {}
# mapping of toolbar object name to the toolbar
self._embed_toolbars = {}
self._signal_mapper_toolbars = QSignalMapper(self)
self._signal_mapper_toolbars.mapped[str].connect(self._on_toolbar_orientation_changed)
def _load(self):
if not Main.main_filename:
raise RuntimeError('PluginHandlerXEmbedContainer._load() filename of initially started script is unknown')
self._dbus_server = Server('tcp:bind=*')
self._dbus_server.on_connection_added.append(self._add_dbus_connection)
self._dbus_container_service = PluginHandlerDBusService(self, self._dbus_object_path)
self._dbus_plugin_settings_service = SettingsProxyDBusService(self._dbus_object_path + '/plugin')
self._dbus_instance_settings_service = SettingsProxyDBusService(self._dbus_object_path + '/instance')
self._process = QProcess(self)
self._process.setProcessChannelMode(QProcess.SeparateChannels)
self._process.readyReadStandardOutput.connect(self._print_process_output)
self._process.readyReadStandardError.connect(self._print_process_error)
self._process.finished.connect(self._emit_close_plugin)
# start python with unbuffered stdout/stderr so that the order of the output is retained
cmd = sys.executable + ' -u'
cmd += ' %s' % Main.main_filename
cmd += ' --qt-binding=%s' % QT_BINDING
cmd += ' --embed-plugin=%s --embed-plugin-serial=%s --embed-plugin-address=%s' % (self.instance_id().plugin_id, self.instance_id().serial_number, self._dbus_server.address)
if self.argv():
cmd += ' --args %s' % ' '.join(self.argv())
#qDebug('PluginHandlerXEmbedContainer._load() starting command: %s' % cmd)
self._process.start(cmd)
started = self._process.waitForStarted(3000)
if not started:
self._dbus_container_service.remove_from_connection()
self._dbus_plugin_settings_service.remove_from_connection()
self._dbus_instance_settings_service.remove_from_connection()
raise RuntimeError('PluginHandlerXEmbedContainer._load() could not start subprocess in reasonable time')
# QProcess.pid() has been added to PySide in 1.0.5
if hasattr(self._process, 'pid'):
self._pid = self._process.pid()
else:
# use serial number as a replacement for pid if not available
self.__class__._serial_number = self._serial_number + 1
self._pid = self._serial_number
qDebug('PluginHandlerXEmbedContainer._load() started subprocess (#%s) for plugin "%s"' % (self._pid, str(self._instance_id)))
# self._emit_load_completed is called asynchronous when client signals finished loading via dbus
def _add_dbus_connection(self, conn):
self._dbus_container_service.add_to_connection(conn, self._dbus_object_path)
self._dbus_plugin_settings_service.add_to_connection(conn, self._dbus_object_path + '/plugin')
self._dbus_instance_settings_service.add_to_connection(conn, self._dbus_object_path + '/instance')
def _print_process_output(self):
self._print_process(self._process.readAllStandardOutput(), qDebug)
def _print_process_error(self):
self._print_process(self._process.readAllStandardError(), qWarning)
def _print_process(self, data, method):
# indent process output and prefix it with the pid
lines = str(data).split('\n')
if lines[-1] == '':
lines.pop()
for line in lines:
method(' %d %s' % (self._pid, line))
def load_completed(self, loaded, has_configuration):
# TODO timer to detect no response
exception = None if loaded else True
self._plugin_has_configuration = has_configuration
self._update_title_bars()
self._emit_load_completed(exception)
def _shutdown_plugin(self):
qDebug('PluginHandlerXEmbedContainer._shutdown_plugin()')
self._process.finished.disconnect(self._emit_close_plugin)
self._dbus_container_service.shutdown_plugin()
def emit_shutdown_plugin_completed(self):
self._dbus_container_service.remove_from_connection()
self._dbus_plugin_settings_service.remove_from_connection()
self._dbus_instance_settings_service.remove_from_connection()
self._process.close()
self._process.waitForFinished(5000)
if self._process.state() != QProcess.NotRunning:
self._process.kill()
self._process = None
super(PluginHandlerXEmbedContainer, self).emit_shutdown_plugin_completed()
def _unload(self):
qDebug('PluginHandlerXEmbedContainer._unload()')
self._emit_unload_completed()
def _save_settings(self, plugin_settings, instance_settings):
qDebug('PluginHandlerXEmbedContainer._save_settings()')
self._dbus_plugin_settings_service.set_settings(plugin_settings)
self._dbus_instance_settings_service.set_settings(instance_settings)
self._dbus_container_service.save_settings()
def emit_save_settings_completed(self):
self._dbus_plugin_settings_service.set_settings(None)
self._dbus_instance_settings_service.set_settings(None)
super(PluginHandlerXEmbedContainer, self).emit_save_settings_completed()
def _restore_settings(self, plugin_settings, instance_settings):
qDebug('PluginHandlerXEmbedContainer._restore_settings()')
self._dbus_plugin_settings_service.set_settings(plugin_settings)
self._dbus_instance_settings_service.set_settings(instance_settings)
self._dbus_container_service.restore_settings()
def emit_restore_settings_completed(self):
self._dbus_plugin_settings_service.set_settings(None)
self._dbus_instance_settings_service.set_settings(None)
super(PluginHandlerXEmbedContainer, self).emit_restore_settings_completed()
def _trigger_configuration(self):
self._dbus_container_service.trigger_configuration()
def embed_widget(self, pid, widget_object_name):
dock_widget = self._create_dock_widget()
embed_container = QX11EmbedContainer(dock_widget)
#embed_container.clientClosed.connect(self._emit_close_signal)
self._add_dock_widget(dock_widget, embed_container)
# update widget title is triggered by client after embedding
self._embed_containers[widget_object_name] = embed_container
return embed_container.winId()
def update_embedded_widget_icon(self, widget_object_name, icon_str):
embed_container = self._embed_containers[widget_object_name]
# deserialize icon base64-encoded string
ba = QByteArray.fromBase64(icon_str)
s = QDataStream(ba, QIODevice.ReadOnly)
icon = QIcon()
s >> icon
embed_container.setWindowIcon(icon)
def update_embedded_widget_title(self, widget_object_name, title):
embed_container = self._embed_containers[widget_object_name]
embed_container.setWindowTitle(title)
def unembed_widget(self, widget_object_name):
embed_container = self._embed_containers[widget_object_name]
self.remove_widget(embed_container)
del self._embed_containers[widget_object_name]
def embed_toolbar(self, pid, toolbar_object_name):
toolbar = QToolBar()
toolbar.setObjectName(toolbar_object_name)
embed_container = QX11EmbedContainer(toolbar)
toolbar.addWidget(embed_container)
#embed_container.clientClosed.connect(self._emit_close_signal)
self._add_toolbar(toolbar)
self._embed_containers[toolbar_object_name] = embed_container
# setup mapping to signal change of orientation to client
self._embed_toolbars[toolbar_object_name] = toolbar
self._signal_mapper_toolbars.setMapping(toolbar, toolbar_object_name)
toolbar.orientationChanged.connect(self._signal_mapper_toolbars.map)
return embed_container.winId()
def _on_toolbar_orientation_changed(self, toolbar_object_name):
embed_container = self._embed_containers[toolbar_object_name]
toolbar = self._embed_toolbars[toolbar_object_name]
self._dbus_container_service.toolbar_orientation_changed(embed_container.winId(), toolbar.orientation() == Qt.Horizontal)
def unembed_toolbar(self, toolbar_object_name):
embed_container = self._embed_containers[toolbar_object_name]
del self._embed_containers[toolbar_object_name]
del self._embed_toolbars[toolbar_object_name]
self.remove_toolbar(embed_container)
embed_container.close()
class PluginManagerWidget(QObject):
_NUM_DESC_ATTRIBUTES = 5
plugin_states_updated_signal = Signal(list)
def __init__(self, context):
super(PluginManagerWidget, self).__init__()
self.namespace = '/'
self.plugin_states_update_sub = None
self.load_plugin_set_client = None
self.get_plugin_descriptions_client = None
self.get_plugin_states_client = None
self.add_plugin_client = None
self.remove_plugin_client = None
self.plugin_descriptions = []
self.add_plugin_selection_filter = PluginDescription()
# start widget
widget = context
vbox = QVBoxLayout()
# load from ui
self.plugin_manager_widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('vigir_plugin_manager'), 'resource', 'plugin_manager.ui')
loadUi(ui_file, self.plugin_manager_widget, {'QWidget': QWidget})
vbox.addWidget(self.plugin_manager_widget)
# init ui
icon = QIcon.fromTheme("view-refresh")
self.plugin_manager_widget.refreshAllPushButton.setIcon(icon)
self.plugin_manager_widget.refreshPluginStatesPushButton.setIcon(icon)
# init tree view
tree_view = self.plugin_manager_widget.plugin_tree_view
tree_view.setSelectionMode(QAbstractItemView.ExtendedSelection)
tree_view.setContextMenuPolicy(Qt.CustomContextMenu)
tree_view.customContextMenuRequested.connect(self._open_context_menu)
self.plugin_tree_model = PluginTreeModel()
tree_view.setModel(self.plugin_tree_model)
# set up combo boxes
self.plugin_manager_widget.pluginNameComboBox.setInsertPolicy(QComboBox.NoInsert)
# references to combo boxes
self.plugin_cb = []
self.plugin_cb.append(self.plugin_manager_widget.pluginNameComboBox)
self.plugin_cb.append(self.plugin_manager_widget.pluginTypeClassComboBox)
self.plugin_cb.append(self.plugin_manager_widget.pluginTypePackageComboBox)
self.plugin_cb.append(self.plugin_manager_widget.pluginBaseClassComboBox)
self.plugin_cb.append(self.plugin_manager_widget.pluginBasePackageComboBox)
# init signal mapper
self.plugin_cb_mapper = QSignalMapper(self)
self.plugin_cb_mapper.mapped.connect(self.add_plugin_selection_changed)
# connect to signals
for i in range(len(self.plugin_cb)):
self.plugin_cb_mapper.setMapping(self.plugin_cb[i], i)
self.plugin_cb[i].currentIndexChanged.connect(self.plugin_cb_mapper.map)
self.plugin_manager_widget.namespaceComboBox.currentIndexChanged[str].connect(self.set_namespace)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(self.search_namespace)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(self.refresh_plugin_descriptions)
self.plugin_manager_widget.refreshAllPushButton.clicked[bool].connect(self.refresh_plugin_states)
self.plugin_manager_widget.loadPluginSetPushButton.clicked[bool].connect(self.load_plugin_set)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[bool].connect(self.refresh_plugin_sets)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[bool].connect(self.refresh_plugin_descriptions)
self.plugin_manager_widget.refreshPluginStatesPushButton.clicked[bool].connect(self.refresh_plugin_states)
self.plugin_manager_widget.addPluginPushButton.clicked[bool].connect(self.add_plugin)
self.plugin_manager_widget.clearAddPluginSelectionPushButton.clicked[bool].connect(self.clear_add_plugin_selection)
self.plugin_manager_widget.removePluginsPushButton.clicked[bool].connect(self.remove_plugins)
# Qt signals
self.plugin_states_updated_signal.connect(self.update_plugin_tree_view)
#self.connect(self, QtCore.SIGNAL('setTransitionModeStatusStyle(PyQt_PyObject)'), self._set_transition_mode_status_style)
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init plugin tree view
self.search_namespace()
def shutdown_plugin(self):
print 'Shutting down ...'
self.plugin_states_update_sub.unregister()
print 'Done!'
def _open_context_menu(self, position):
indexes = self.plugin_manager_widget.plugin_tree_view.selectedIndexes()
level = -1
if len(indexes) > 0:
level = 0
index = indexes[0]
while index.parent().isValid():
index = index.parent()
level += 1
menu = QMenu()
if level == 0:
expand_action = QAction(self.tr('Expand'), None)
expand_action.triggered.connect(self.plugin_manager_widget.plugin_tree_view.expandAll)
menu.addAction(expand_action)
if level == 0 or level == 1:
remove_action = QAction(self.tr('Remove'), None)
remove_action.triggered.connect(self.remove_plugins)
menu.addAction(remove_action)
menu.exec_(self.plugin_manager_widget.plugin_tree_view.viewport().mapToGlobal(position))
def init_topics(self, namespace):
# init subscribers
self.plugin_states_update_sub = rospy.Subscriber(namespace + 'plugin_manager/plugin_states_update', PluginStates, self.plugin_states_update)
# init action clients
self.load_plugin_set_client = actionlib.SimpleActionClient(namespace + 'plugin_manager/load_plugin_set', PluginManagementAction)
self.get_plugin_descriptions_client = actionlib.SimpleActionClient(namespace + 'plugin_manager/get_plugin_descriptions', GetPluginDescriptionsAction)
self.get_plugin_states_client = actionlib.SimpleActionClient(namespace + 'plugin_manager/get_plugin_states', GetPluginStatesAction)
self.add_plugin_client = actionlib.SimpleActionClient(namespace + 'plugin_manager/add_plugin', PluginManagementAction)
self.remove_plugin_client = actionlib.SimpleActionClient(namespace + 'plugin_manager/remove_plugin', PluginManagementAction)
print("Switched to namespace '" + namespace + "'")
def _set_data_in_description(self, description, index, data):
if index == 0:
description.name.data = data
if index == 1:
description.type_class.data = data
if index == 2:
description.type_class_package.data = data
if index == 3:
description.base_class.data = data
if index == 4:
description.base_class_package.data = data
return description
def _get_data_from_description(self, description, index):
if index == 0:
return description.name.data
if index == 1:
return description.type_class.data
if index == 2:
return description.type_class_package.data
if index == 3:
return description.base_class.data
if index == 4:
return description.base_class_package.data
def filter_descriptions(self, filtered_list, description_filter):
result = filtered_list
for i in range(self._NUM_DESC_ATTRIBUTES):
if not self._get_data_from_description(description_filter, i):
continue
result = filter(lambda d: self._get_data_from_description(description_filter, i) == self._get_data_from_description(d, i), result)
return result
@Slot()
def search_namespace(self):
cb = self.plugin_manager_widget.namespaceComboBox
cb.blockSignals(True)
cb.setEnabled(False)
cb.clear()
cb.addItem('Updating...')
# get topic list
_, _, topic_type = rospy.get_master().getTopicTypes()
topic_dict = dict(topic_type)
# filter list
topic_dict_filtered = dict()
for k, v in topic_dict.items():
if v == 'vigir_pluginlib_msgs/GetPluginStatesActionGoal':
topic_dict_filtered[k] = v
# update combo box with found namespaces
cb.clear()
namespaces = [ns[:-37] for ns in sorted(topic_dict_filtered.keys())]
cb.addItems(namespaces)
if cb.count() > 0:
self.set_namespace(cb.currentText())
cb.setEnabled(True)
cb.blockSignals(False)
else:
cb.addItem('No topics available!')
@Slot(str)
def set_namespace(self, namespace):
self.namespace = namespace
self.init_topics(namespace)
self.refresh_plugin_sets()
self.refresh_plugin_descriptions()
self.refresh_plugin_states()
@Slot()
def refresh_plugin_sets(self):
plugin_sets = []
if rospy.has_param(self.namespace+'/plugin_sets'):
plugin_sets = rospy.get_param(self.namespace+'/plugin_sets').keys()
cb = self.plugin_manager_widget.loadPluginSetComboBox
cb.clear()
if plugin_sets:
cb.addItems(plugin_sets)
cb.setEnabled(True)
self.plugin_manager_widget.loadPluginSetPushButton.setEnabled(True)
else:
cb.setEnabled(False)
self.plugin_manager_widget.loadPluginSetPushButton.setEnabled(False)
@Slot()
def load_plugin_set(self):
if self.load_plugin_set_client.wait_for_server(rospy.Duration(1.0)):
# send request to server
goal = PluginManagementGoal()
goal.name.data = self.plugin_manager_widget.loadPluginSetComboBox.currentText()
self.load_plugin_set_client.send_goal(goal)
@Slot(int)
def add_plugin_selection_changed(self, index):
# update filter mask
self._set_data_in_description(self.add_plugin_selection_filter, index, self.plugin_cb[index].currentText())
# block signals of combo boxes
for cb in self.plugin_cb:
cb.blockSignals(True)
# filter elements in combo boxes
filtered_descriptions = self.filter_descriptions(self.plugin_descriptions, self.add_plugin_selection_filter)
for cb_index in range(self._NUM_DESC_ATTRIBUTES):
if cb_index != index:
rows_enabled = 0
last_enabled_row_index = 0
data = [self._get_data_from_description(d, cb_index) for d in filtered_descriptions]
item_texts = [self.plugin_cb[cb_index].itemText(i) for i in range(self.plugin_cb[cb_index].count())]
for row in range(1, len(item_texts)):
if not item_texts[row] or item_texts[row] in data:
self.plugin_cb[cb_index].setItemData(row, 33, Qt.UserRole - 1) # enable item
rows_enabled += 1
last_enabled_row_index = row
else:
self.plugin_cb[cb_index].setItemData(row, 0, Qt.UserRole - 1) # disable item
# if only one element is left, then auto select it
if rows_enabled == 1:
self.plugin_cb[cb_index].setCurrentIndex(last_enabled_row_index)
# unblock signals of combo boxes
for cb in self.plugin_cb:
cb.blockSignals(False)
self.plugin_manager_widget.addPluginPushButton.setEnabled(True)
@Slot()
def clear_add_plugin_selection(self):
# block signals of combo boxes
for cb in self.plugin_cb:
cb.blockSignals(True)
self.plugin_cb[0].clearEditText()
for cb in self.plugin_cb:
cb.setCurrentIndex(0)
# reset selection filter
self.add_plugin_selection_filter = PluginDescription()
for cb in self.plugin_cb:
for row in range(cb.count()):
cb.setItemData(row, 33, Qt.UserRole - 1) # enable item
# unblock signals of combo boxes
for cb in self.plugin_cb:
cb.blockSignals(False)
self.plugin_manager_widget.addPluginPushButton.setEnabled(False)
@Slot()
def refresh_plugin_descriptions(self):
# clear old status
self.plugin_descriptions = []
for cb in self.plugin_cb:
cb.blockSignals(True)
cb.clear()
self.plugin_manager_widget.addPluginPushButton.setEnabled(False)
# collect all plugin descriptions from manager
if self.get_plugin_descriptions_client.wait_for_server(rospy.Duration(1.0)):
self.get_plugin_descriptions_client.send_goal(GetPluginDescriptionsGoal())
if self.get_plugin_descriptions_client.wait_for_result(rospy.Duration(5.0)):
self.plugin_descriptions = self.get_plugin_descriptions_client.get_result().descriptions
# collect all plugins loaded into param server
all_params = rosparam.list_params(self.namespace)
for pname in all_params:
# remove the plugin manager namespace
if self.namespace == '/':
pname_sub = pname
else:
pname_sub = pname[len(self.namespace):]
psplit = pname_sub.split('/')
# get plugin description from param server
if len(psplit) >= 2 and psplit[1] == 'type_class':
description = PluginDescription()
description.name.data = psplit[0]
description.type_class.data = rospy.get_param(pname)
if rospy.has_param(self.namespace+psplit[0]+'/type_class_package'):
description.type_class_package.data = rospy.get_param(self.namespace+psplit[0]+'/type_class_package')
if rospy.has_param(self.namespace+psplit[0]+'/base_class'):
description.base_class.data = rospy.get_param(self.namespace+psplit[0]+'/base_class')
if rospy.has_param(self.namespace+psplit[0]+'/base_class_package'):
description.base_class_package.data = rospy.get_param(self.namespace+psplit[0]+'/base_class_package')
self.plugin_descriptions.append(description)
# prepare combo box item texts
description = [[''] for i in range(self._NUM_DESC_ATTRIBUTES)]
for pd in self.plugin_descriptions:
for i in range(self._NUM_DESC_ATTRIBUTES):
description[i].append(self._get_data_from_description(pd, i))
# update combo boxes
for i in range(len(self.plugin_cb)):
description[i] = sorted(list(set(description[i])))
self.plugin_cb[i].addItems(description[i])
for cb in self.plugin_cb:
cb.blockSignals(False)
@Slot()
def refresh_plugin_states(self):
if self.get_plugin_states_client.wait_for_server(rospy.Duration(1.0)):
self.get_plugin_states_client.send_goal(GetPluginStatesGoal())
if self.get_plugin_states_client.wait_for_result(rospy.Duration(5.0)):
self.plugin_states_updated_signal.emit(self.get_plugin_states_client.get_result().states)
@Slot(PluginStates)
def plugin_states_update(self, plugin_states_msg):
self.plugin_states_updated_signal.emit(plugin_states_msg.states)
@Slot(list)
def update_plugin_tree_view(self, states):
self.plugin_tree_model.updateData(states)
#self.plugin_manager_widget.plugin_tree_view.setModel(self.plugin_tree_model)
#for column in range(0, self.plugin_tree_model.columnCount()):
# self.plugin_tree_model.resizeColumnToContents(column)
#self.plugin_manager_widget.plugin_tree_view.expandAll()
@Slot()
def add_plugin(self):
if self.add_plugin_client.wait_for_server(rospy.Duration(1.0)):
# generate plugin description
description = PluginDescription()
for i in range(self._NUM_DESC_ATTRIBUTES):
self._set_data_in_description(description, i, self.plugin_cb[i].currentText())
# send request to server
goal = PluginManagementGoal()
goal.descriptions.append(description)
self.add_plugin_client.send_goal(goal)
@Slot()
def remove_plugins(self):
indexes = self.plugin_manager_widget.plugin_tree_view.selectionModel().selectedIndexes()
indexes = filter(lambda index: index.column() == 0, indexes)
# extract plugin descriptions from selection
descriptions = []
for index in indexes:
descriptions.append(index.internalPointer().getPluginState().description)
# send request to server
if self.remove_plugin_client.wait_for_server(rospy.Duration(1.0)):
goal = PluginManagementGoal()
goal.descriptions = descriptions
self.remove_plugin_client.send_goal(goal)
