class GroundStationWidget(QWidget):
def __init__(self):
super(GroundStationWidget, self).__init__()
self._principle_layout = QBoxLayout(0) # main layout is horizontal (0)
self._principle_layout = QSplitter(Qt.Horizontal)
self._map_layout = QVBoxLayout()
map_widget = QWidget()
map_widget.setLayout(self._map_layout)
self._principle_layout.addWidget(map_widget)
self._control_layout = QVBoxLayout()
control_widget = QWidget()
control_widget.setLayout(self._control_layout)
self._principle_layout.addWidget(control_widget)
self.setAcceptDrops(False) # Dragging and Dropping not permitted
self.setWindowTitle('ROSplane Ground Station')
#=============================
self._mw = MapWindow()
self._map_layout.addWidget(self._mw)
self._tv = PlotWidget()
self._data_plot = DataPlot(self._tv)
self._data_plot.set_autoscale(x=False)
self._data_plot.set_autoscale(y=DataPlot.SCALE_EXTEND
| DataPlot.SCALE_VISIBLE)
self._data_plot.set_xlim([0, 10.0])
self._tv.switch_data_plot_widget(self._data_plot)
self._control_layout.addWidget(
self._tv,
1) # ratio of these numbers determines window proportions
self._ah = ArtificialHorizon()
self._control_layout.addWidget(self._ah, 1)
#=============================
print('fake init')
total_layout = QBoxLayout(QBoxLayout.TopToBottom)
self._principle_layout.setHandleWidth(8)
total_layout.addWidget(self._principle_layout)
self.setLayout(total_layout)
# Global timer for marble_map and artificial_horizon
self.interval = 100 # in milliseconds, period of regular update
self.timer = QTimer(self)
self.timer.setInterval(self.interval)
self.timer.timeout.connect(self._mw._marble_map.update)
self.timer.timeout.connect(self._ah.update)
self.timer.start()
def closeEvent(self, event):
self.timer.stop()
def save_settings(self, plugin_settings,
instance_settings): # have a file to read and write from
print('fake save') # < prints to terminal
def restore_settings(self, plugin_settings, instance_settings):
print('fake restore')
class Visualizer(Plugin):
def __init__(self, context):
super(Visualizer, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Visualizer')
# Process standalone plugin command-line arguments
# from argparse import ArgumentParser
# parser = ArgumentParser()
# Add argument(s) to the parser.
# parser.add_argument("-q", "--quiet", action="store_true",
# dest="quiet",
# help="Put plugin in silent mode")
# args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('consai2_gui'),
'resource', 'Visualizer.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget, {"PaintWidget": PaintWidget})
# Give QObjects reasonable names
self._widget.setObjectName('VisualizerUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self._redraw_timer = QTimer()
self._redraw_timer.setInterval(16)
self._redraw_timer.timeout.connect(self._widget.update)
self._redraw_timer.start()
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class RosPlot(FigureCanvas):
"""Ultimately, this is a QWidget (as well as a FigureCanvasAgg, etc.)."""
def __init__(self, parent, label_name, topic_name, topic_type, topic_field,
buffer_size):
self.label_name = label_name
self.topic_type = topic_type
self.topic_name = topic_name
self.topic_field = topic_field
self.buffer_size = buffer_size
self.timer = QTimer()
self.timer.setInterval(100)
self.timer.timeout.connect(self.update_figure)
fig = Figure(figsize=(5, 4), dpi=100)
self.axes = fig.add_subplot(111)
# We want the axes cleared every time plot() is called
self.axes.hold(False)
self.compute_initial_figure()
self.buffer = collections.deque(maxlen=self.buffer_size)
FigureCanvas.__init__(self, fig)
self.setParent(parent)
FigureCanvas.setSizePolicy(self, QtGui.QSizePolicy.Expanding,
QtGui.QSizePolicy.Expanding)
FigureCanvas.updateGeometry(self)
self.subscriber = RosHelper.create_subscriber_from_type(
self.topic_name, self.topic_type, self.on_message)
self.timer.start()
def compute_initial_figure(self):
pass
def on_message(self, msg):
r = msg
for subfields in self.topic_field.split(".")[1:]:
r = getattr(r, subfields)
self.buffer.append(r)
def update_figure(self):
x = np.array(range(0, len(self.buffer)))
y = np.array(self.buffer)
self.axes.plot(x, y.T, 'r')
self.draw()
class RosPlot(FigureCanvas):
"""Ultimately, this is a QWidget (as well as a FigureCanvasAgg, etc.)."""
def __init__(self, parent, label_name,topic_name,topic_type,topic_field,buffer_size):
self.label_name= label_name
self.topic_type = topic_type
self.topic_name = topic_name
self.topic_field = topic_field
self.buffer_size = buffer_size
self.timer = QTimer()
self.timer.setInterval(100)
self.timer.timeout.connect(self.update_figure)
fig = Figure(figsize=(5, 4), dpi=100)
self.axes = fig.add_subplot(111)
# We want the axes cleared every time plot() is called
self.axes.hold(False)
self.compute_initial_figure()
self.buffer = collections.deque(maxlen=self.buffer_size)
FigureCanvas.__init__(self, fig)
self.setParent(parent)
FigureCanvas.setSizePolicy(self,
QtGui.QSizePolicy.Expanding,
QtGui.QSizePolicy.Expanding)
FigureCanvas.updateGeometry(self)
self.subscriber = RosHelper.create_subscriber_from_type(self.topic_name,self.topic_type,self.on_message)
self.timer.start()
def compute_initial_figure(self):
pass
def on_message(self,msg):
r = msg
for subfields in self.topic_field.split(".")[1:]:
r = getattr(r,subfields)
self.buffer.append(r)
def update_figure(self):
x = np.array(range(0,len(self.buffer)))
y = np.array(self.buffer)
self.axes.plot(x, y.T, 'r')
self.draw()
class Visualizer(Plugin):
def __init__(self, context):
super(Visualizer, self).__init__(context)
self.setObjectName("Visualizer")
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path("sai_visualizer"), \
"resource", "visualizer_widget.ui")
loadUi(ui_file, self._widget, {"PaintWidget": PaintWidget})
self._widget.setObjectName("VisualizerUi")
self._widget.setWindowTitle("SAI Visualizer")
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self._redraw_timer = QTimer()
self._redraw_timer.setInterval(16)
self._redraw_timer.timeout.connect(self._widget.update)
self._redraw_timer.start()
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class Conman(Plugin):
update_graph_sig = Signal(str)
def __init__(self, context):
super(Conman, self).__init__(context)
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Conman')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'rqt_conman.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.ns_refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
self._widget.setObjectName('ConmanPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
palette = QPalette()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#self._widget.subscribe_button.setCheckable(True)
self._widget.namespace_input.currentIndexChanged.connect(
self._handle_refresh_clicked)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.refresh_button.clicked[bool].connect(
self._handle_refresh_clicked)
self._widget.commit_button.clicked[bool].connect(
self._handle_commit_clicked)
#self._widget.xdot_widget.connect(
#self._widget.xdot_widget, SIGNAL('_update_graph'), self._widget.xdot_widget.set_dotcode)
self.update_graph_sig.connect(self._update_graph)
self.blocks = {}
self.groups = {}
self._ns = ""
self._actions_connected = False
self.enable_widgets(False)
self.new_dotcode_data = ''
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widgets)
#self.update_timer.start()
self._get_blocks = None
self._set_blocks = None
self._blocks_model = QStandardItemModel(0, 4)
self._blocks_model.setHeaderData(0, Qt.Horizontal, "")
self._blocks_model.setHeaderData(1, Qt.Horizontal, "Action")
self._blocks_model.setHeaderData(2, Qt.Horizontal, "State")
self._blocks_model.setHeaderData(3, Qt.Horizontal, "Block")
self._widget.blocks_table.setModel(self._blocks_model)
self._blocks_delegate = BlocksDelegate(self)
self._widget.blocks_table.setItemDelegate(self._blocks_delegate)
self._blocks_model.itemChanged.connect(self.block_changed)
self._groups_model = QStandardItemModel(0, 4)
self._groups_model.setHeaderData(0, Qt.Horizontal, "")
self._groups_model.setHeaderData(1, Qt.Horizontal, "")
self._groups_model.setHeaderData(2, Qt.Horizontal, "")
self._groups_model.setHeaderData(3, Qt.Horizontal, "Group")
self._widget.groups_table.setModel(self._groups_model)
self._groups_delegate = GroupsDelegate(self)
self._widget.groups_table.setItemDelegate(self._groups_delegate)
self.refresh_combo_box()
def block_changed(self, item):
row = item.row()
name = self._blocks_model.item(row, 3).text()
block = self.blocks[name]
checked = item.checkState() == Qt.Checked
def shutdown_plugin(self):
# TODO unregister all publishers here
self._update_thread.kill()
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
#def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
@Slot()
def refresh_combo_box(self):
self._update_thread.kill()
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('updating...')
self._update_thread.start()
def _update_thread_run(self):
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
keys = list(self._topic_dict.keys())
namespaces = list()
for i in keys:
if i.endswith("get_blocks_action/goal"):
namespaces.append(i[0:i.index("get_blocks_action/goal")])
self._widget.namespace_input.setItems.emit(namespaces)
@Slot()
def _update_finished(self):
self._widget.namespace_input.setEnabled(True)
def _get_result_cb(self, status, res):
rospy.loginfo("got result!")
self._blocks_model.setRowCount(0)
self._blocks_model.setRowCount(len(res.blocks))
for (i, block) in zip(range(len(res.blocks)), res.blocks):
# Store in dict
self.blocks[block.name] = block
cb = QStandardItem(True)
cb.setCheckable(True)
cb.setCheckState(Qt.Checked if block.state.value == conman_msgs.
msg.TaskState.RUNNING else Qt.Unchecked)
cb.setTextAlignment(Qt.AlignHCenter)
cb.setTristate(True)
action = QStandardItem(True)
action.setText("")
state = QStandardItem(True)
state.setText(state_map[block.state.value])
name = QStandardItem(True)
name.setText(str(block.name))
self._blocks_model.setItem(i, 0, cb)
self._blocks_model.setItem(i, 1, action)
self._blocks_model.setItem(i, 2, state)
self._blocks_model.setItem(i, 3, name)
for (i, group) in zip(range(len(res.groups)), res.groups):
self.groups[group.name] = group
cb = QStandardItem(True)
cb.setCheckable(True)
cb.setCheckState(Qt.Checked)
cb.setTextAlignment(Qt.AlignHCenter)
cb.setEnabled(False)
name = QStandardItem(True)
name.setText(str(group.name))
self._groups_model.setItem(i, 0, cb)
self._groups_model.setItem(i, 3, name)
self._update_groups()
self._update_blocks()
self._widget.blocks_table.resizeColumnsToContents()
self._widget.blocks_table.horizontalHeader().setStretchLastSection(
True)
self._widget.groups_table.resizeColumnsToContents()
self._widget.groups_table.horizontalHeader().setStretchLastSection(
True)
def _update_blocks(self):
for (name, block) in self.blocks.items():
items = self._blocks_model.findItems(name, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
checked = self._blocks_model.item(row,
0).checkState() == Qt.Checked
if checked and block.state.value != conman_msgs.msg.TaskState.RUNNING:
self._blocks_model.item(row, 1).setText("ENABLE")
elif not checked and block.state.value == conman_msgs.msg.TaskState.RUNNING:
self._blocks_model.item(row, 1).setText("DISABLE")
else:
self._blocks_model.item(row, 1).setText("")
self._update_groups()
def _enable_group(self, index, enable):
name = self._groups_model.item(index, 3).text()
group = self.groups[name]
for member in group.members:
items = self._blocks_model.findItems(member, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
self._blocks_model.item(
row, 0).setCheckState(Qt.Checked if enable else Qt.Unchecked)
self._update_blocks()
def _update_groups(self):
for (name, group) in self.groups.items():
group_items = self._groups_model.findItems(name, column=3)
if len(group_items) > 0:
group_item = group_items[0]
else:
continue
group_row = group_item.row()
members_checked = []
for member in group.members:
items = self._blocks_model.findItems(member, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
members_checked.append(
self._blocks_model.item(row, 0).checkState() == Qt.Checked)
if all(members_checked):
check = Qt.Checked
else:
check = Qt.Unchecked
self._groups_model.item(group_row, 0).setCheckState(check)
def _query_blocks(self):
if self._get_blocks and self._actions_connected:
if self._get_blocks.simple_state == actionlib.SimpleGoalState.DONE:
rospy.loginfo("Getting blocks...")
goal = conman_msgs.msg.GetBlocksGoal()
goal.publish_flow_graph = self._widget.generate_graph_checkbox.isChecked(
)
self._get_blocks.send_goal(goal, done_cb=self._get_result_cb)
def enable_widgets(self, enable):
#self._widget.generate_graph_checkbox.setEnabled(enable)
self._widget.force_enable_checkbox.setEnabled(enable)
#self._widget.disable_unused_button.setEnabled(enable)
self._widget.xdot_widget.setEnabled(enable)
self._widget.blocks_table.setEnabled(enable)
self._widget.groups_table.setEnabled(enable)
self._widget.regenerate_graph_button.setEnabled(enable)
def _handle_refresh_clicked(self, checked):
ns = self._widget.namespace_input.currentText()
if len(ns) > 0:
if self._ns != ns:
self._actions_connected = False
self._ns = ns
self.enable_widgets(False)
self._dotcode_sub = rospy.Subscriber(ns + '/dotcode',
std_msgs.msg.String,
self._dotcode_msg_cb)
self._get_blocks = actionlib.SimpleActionClient(
ns + '/get_blocks_action', conman_msgs.msg.GetBlocksAction)
self._set_blocks = actionlib.SimpleActionClient(
ns + '/set_blocks_action', conman_msgs.msg.SetBlocksAction)
rospy.loginfo("Creating action clients on namespace '%s'..." %
ns)
if not self._get_blocks.wait_for_server(rospy.Duration(2)):
rospy.loginfo("Timed out waiting for %s." %
self._get_blocks.action_client.ns)
return
if not self._set_blocks.wait_for_server(rospy.Duration(2)):
rospy.loginfo("Timed out waiting for %s." %
self._set_blocks.action_client.ns)
return
rospy.loginfo("Action clients created.")
self._actions_connected = True
self.enable_widgets(True)
self._query_blocks()
def _handle_commit_clicked(self, checked):
if self._set_blocks and self._actions_connected:
if self._get_blocks.simple_state == actionlib.SimpleGoalState.DONE:
rospy.loginfo("Setting blocks...")
goal = conman_msgs.msg.SetBlocksGoal()
goal.diff = True
goal.force = True
for i in range(self._blocks_model.rowCount()):
name = self._blocks_model.item(i, 3).text()
action = self._blocks_model.item(i, 1).text()
if action == 'DISABLE':
goal.disable.append(name)
elif action == 'ENABLE':
goal.enable.append(name)
self._set_blocks.send_goal(goal,
done_cb=self._get_set_result_cb)
def _get_set_result_cb(self, status, res):
self._query_blocks()
@Slot(str)
def _update_graph(self, dotcode):
global initialized
if initialized:
self._widget.xdot_widget.set_dotcode(dotcode, center=False)
else:
self._widget.xdot_widget.set_dotcode(dotcode, center=True)
self._widget.xdot_widget.zoom_to_fit()
initialized = 1
def _dotcode_msg_cb(self, msg):
#self.new_dotcode_data = msg.data
self.update_graph_sig.emit(msg.data)
def _update_widgets(self):
self._update_groups()
self._update_blocks()
class Top(Plugin):
NODE_FIELDS = [
'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'
]
OUT_FIELDS = [
'node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads'
]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s']
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads']
SORT_TYPE = [str, str, float, float, float]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x:
('Resident: %0.2f MiB, Virtual: %0.2f MiB' %
(x[0] / 2**20, x[1] / 2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.items():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
twi.setHidden(len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex',
int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class MotionEditorWidget(QWidget):
position_renamed = Signal(QListWidgetItem)
def __init__(self, motion_publisher, robot_config):
super(MotionEditorWidget, self).__init__()
self.robot_config = robot_config
self._motion_publisher = motion_publisher
self._motion_data = MotionData(robot_config)
self._filter_pattern = ''
self._playback_marker = None
self._playback_timer = None
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'motion_editor.ui')
loadUi(ui_file, self)
self.list_widgets = {}
for group_type in robot_config.group_types():
list_widget = QListWidget()
list_widget.setSortingEnabled(True)
list_widget.setDragDropMode(QAbstractItemView.DragOnly)
list_widget.setContextMenuPolicy(Qt.CustomContextMenu)
list_widget.customContextMenuRequested.connect(
lambda pos, _group_type=group_type: self.
positions_list_context_menu(_group_type, pos))
list_widget.itemChanged.connect(self.on_list_item_changed)
self.position_lists_layout.addWidget(list_widget)
self.list_widgets[group_type] = list_widget
self._timeline_widget = TimelineWidget()
for track_name in self.robot_config.sorted_groups():
track_type = self.robot_config.groups[track_name].group_type
track = self._timeline_widget.add_track(track_name, track_type)
list_widget = self.list_widgets[track_type]
palette = list_widget.palette()
palette.setColor(QPalette.Base, track._colors['track'])
list_widget.setPalette(palette)
self.timeline_group.layout().addWidget(self._timeline_widget)
for group_type in robot_config.group_types():
label = QLabel(group_type)
label.setAlignment(Qt.AlignHCenter | Qt.AlignVCenter)
self.group_label_layout.addWidget(label)
self.update_motion_name_combo()
self.stop_motion_button.pressed.connect(self.on_motion_stop_pressed)
def on_list_item_changed(self, item):
print 'Position name changed from', item._text, 'to', item.text()
self.position_renamed.emit(item)
def on_motion_stop_pressed(self):
self._clear_playback_marker()
self._motion_publisher.stop_motion()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('splitter', self.splitter.saveState())
instance_settings.set_value('filter_pattern',
self.filter_pattern_edit.text())
instance_settings.set_value('filter_motions_checked',
self.filter_motions_check.isChecked())
instance_settings.set_value('filter_positions_checked',
self.filter_positions_check.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.contains('splitter'):
self.splitter.restoreState(instance_settings.value('splitter'))
else:
self.splitter.setSizes([300, 100])
self.filter_pattern_edit.setText(
instance_settings.value('filter_pattern', ''))
self.filter_motions_check.setChecked(
instance_settings.value('filter_motions_checked', False) in (
1, True, 'true'))
self.filter_positions_check.setChecked(
instance_settings.value('filter_positions_checked', False) in (
1, True, 'true'))
@Slot()
def on_clear_motion_button_clicked(self):
self._timeline_widget.scene().clear()
@Slot()
def on_delete_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error', 'No motion selected to delete.')
return
if motion_name not in self._motion_data:
QMessageBox.warning(
self, 'Error', 'Motion "%s" not in motion list.' % motion_name)
return
self._motion_data.delete(motion_name)
self.update_motion_name_combo()
@Slot()
def on_save_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error',
'No name given to save this motion.')
return
self._motion_data.save(motion_name, self.get_motion_from_timeline())
self.update_motion_name_combo()
@Slot()
def on_load_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error', 'No motion selected to load.')
return
if motion_name not in self._motion_data:
QMessageBox.warning(
self, 'Error', 'Motion "%s" not in motion list.' % motion_name)
return
self.on_clear_motion_button_clicked()
motion = self._motion_data[motion_name]
for group_name, poses in motion.items():
for pose in poses:
data = self.robot_config.groups[group_name].adapt_to_side(
pose['positions'])
self._timeline_widget.scene().add_clip(group_name,
pose['name'],
pose['starttime'],
pose['duration'], data)
@Slot()
def on_null_motion_button_clicked(self):
self._clear_playback_marker()
for group_name in self.robot_config.groups:
target_position = [0] * len(
self.robot_config.groups[group_name].joints)
self._motion_publisher.move_to_position(
group_name, target_position, self.time_factor_spin.value())
@Slot()
def on_run_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error', 'No motion selected to run.')
return
if motion_name not in self._motion_data:
QMessageBox.warning(
self, 'Error', 'Motion "%s" not in motion list.' % motion_name)
return
self._clear_playback_marker()
self._motion_publisher.publish_motion(self._motion_data[motion_name],
self.time_factor_spin.value())
print '[Motion Editor] Running motion:', motion_name
@Slot(str)
def on_filter_pattern_edit_textChanged(self, pattern):
self._filter_pattern = pattern
self._apply_filter_to_position_lists()
self.update_motion_name_combo()
def _apply_filter_to_position_lists(self):
for group_type in self.robot_config.group_types():
list_widget = self.list_widgets[group_type]
for row in range(list_widget.count()):
item = list_widget.item(row)
hidden = self.filter_positions_check.isChecked() and re.search(
self._filter_pattern, item.text()) is None
item.setHidden(hidden)
@Slot(bool)
def on_filter_positions_check_toggled(self, checked):
self._apply_filter_to_position_lists()
@Slot(bool)
def on_filter_motions_check_toggled(self, checked):
self.update_motion_name_combo()
def update_motion_name_combo(self):
combo = self.motion_name_combo
# remember selected motion name
motion_name = str(combo.currentText())
# update motion names
combo.clear()
motion_names = self._motion_data.keys()
if self.filter_motions_check.isChecked():
motion_names = [
name for name in motion_names
if re.search(self._filter_pattern, name) is not None
]
combo.addItems(sorted(motion_names))
# restore previously selected motion name
motion_index = combo.findText(motion_name)
combo.setCurrentIndex(motion_index)
def update_positions_lists(self, positions):
for group_type in self.robot_config.group_types():
list_widget = self.list_widgets[group_type]
list_widget.clear()
for name, position in positions[group_type].items():
item = QListWidgetItem(name)
item._data = position
item._text = name
item._type = group_type
item.setFlags(item.flags() | Qt.ItemIsEditable)
list_widget.addItem(item)
self._apply_filter_to_position_lists()
def positions_list_context_menu(self, group_type, pos):
list_widget = self.list_widgets[group_type]
list_item = list_widget.itemAt(pos)
if list_item is None:
return
menu = QMenu()
move_to = {}
for group in self.robot_config.group_list():
if list_item._type == group.group_type:
move_to[menu.addAction('move "%s"' %
group.name)] = [group.name]
# move_to[menu.addAction('move all')] = list(move_to.itervalues())
move_to[menu.addAction('move all')] = [
group_list[0] for group_list in list(move_to.itervalues())
]
result = menu.exec_(list_widget.mapToGlobal(pos))
if result in move_to:
group_names = move_to[result]
for group_name in group_names:
target_positions = self.robot_config.groups[
group_name].adapt_to_side(list_item._data)
self._motion_publisher.move_to_position(
group_name, target_positions,
self.time_factor_spin.value())
print '[Motion Editor] Moving %s to: %s' % (
group_name,
zip(self.robot_config.groups[group_name].joints_sorted(),
target_positions))
def get_motion_from_timeline(self):
motion = {}
for group_name, clips in self._timeline_widget.scene().clips().items():
motion[group_name] = []
for clip in clips:
pose = {
'name':
clip.text(),
'starttime':
clip.starttime(),
'duration':
clip.duration(),
'positions':
self.robot_config.groups[group_name].adapt_to_side(
clip.data()),
}
motion[group_name].append(pose)
return motion
@Slot()
def on_run_timeline_button_clicked(self):
print '[Motion Editor] Running timeline.'
self._playback_duration = 0.0
for clips in self._timeline_widget.scene().clips().values():
if len(clips) > 0 and self._playback_duration < clips[-1].endtime(
):
self._playback_duration = clips[-1].endtime()
self._playback_time_factor = self.time_factor_spin.value()
self._clear_playback_marker()
self._playback_marker = self._timeline_widget.scene().add_marker(0.0)
self._playback_timer = QTimer()
self._playback_timer.setInterval(30)
self._playback_timer.timeout.connect(self._playback_update)
self._playback_start = rospy.get_time()
self._motion_publisher.publish_motion(self.get_motion_from_timeline(),
self.time_factor_spin.value())
self._playback_timer.start()
def _clear_playback_marker(self):
if self._playback_timer is not None:
self._playback_timer.stop()
if self._playback_marker is not None:
self._playback_marker.remove()
@Slot()
def _playback_update(self):
duration = (rospy.get_time() -
self._playback_start) / self._playback_time_factor
if duration > self._playback_duration:
self._clear_playback_marker()
else:
self._playback_marker.set_time(duration)
class View(QWidget):
def __init__(self, parent=None):
super(View, self).__init__()
self.unit = 'deg'
self.timer = QTimer(self)
self.timer.timeout.connect(lambda: self.publishRequested.emit())
pkg_path = rospkg.RosPack().get_path('rqt_tf_rot')
ui_file_path = os.path.join(pkg_path, 'res', 'PluginUI.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file_path, self)
self.initUI()
publishRequested = Signal()
def initUI(self):
btnPublish = self.findChild(QAbstractButton, 'btnPublish')
btnPublish.clicked.connect(lambda: self.publishRequested.emit())
self.txtChild = self.findChild(QLineEdit, 'txtChild')
self.cboParent = self.findChild(QComboBox, 'cboParent')
self.spnRollRad = self.findChild(QDoubleSpinBox, 'spnRollRad')
self.spnPitchRad = self.findChild(QDoubleSpinBox, 'spnPitchRad')
self.spnYawRad = self.findChild(QDoubleSpinBox, 'spnYawRad')
spnTimeout = self.findChild(QSpinBox, 'spnTimeout')
spnTimeout.valueChanged.connect(
lambda timeout: self.timer.setInterval(timeout))
chkTimer = self.findChild(QCheckBox, 'chkTimer')
chkTimer.stateChanged.connect(self.onChkTimerChanged)
for suffix in ['Yaw', 'Pitch', 'Roll']:
degSpin = self.findChild(QDoubleSpinBox, 'spn'+suffix+'Deg')
radSpin = self.findChild(QDoubleSpinBox, 'spn'+suffix+'Rad')
dial = self.findChild(QDial, 'dial'+suffix)
# May the Python gods forgive me
# The lambda default parameters are there only to simulate
# what would happen if lambdas remembered what objects
# they were trying to capture instead of just the names of
# the variables in the closure
dial.valueChanged.connect(
lambda value, degSpin=degSpin, radSpin=radSpin:
self.onDialChanged(value,
deg_spinbox=degSpin,
rad_spinbox=radSpin))
degSpin.valueChanged.connect(
lambda value, dial=dial, radSpin=radSpin:
self.onDegSpinChanged(
value,
slider=dial,
rad_spinbox=radSpin))
radSpin.valueChanged.connect(
lambda value, dial=dial, degSpin=degSpin:
self.onRadSpinChanged(value,
slider=dial,
deg_spinbox=degSpin))
# --- Slots (not all strictly Qt slots)
def onDialChanged(self, value, deg_spinbox=None, rad_spinbox=None):
setValueWithoutSignals(deg_spinbox, float(value))
setValueWithoutSignals(rad_spinbox,
convertUnit(float(value), 'deg', 'rad'))
def onDegSpinChanged(self, value, slider=None, rad_spinbox=None):
setValueWithoutSignals(slider, int(value))
setValueWithoutSignals(rad_spinbox,
convertUnit(float(value), 'deg', 'rad'))
def onRadSpinChanged(self, value, slider=None, deg_spinbox=None):
setValueWithoutSignals(slider, int(value))
setValueWithoutSignals(deg_spinbox,
convertUnit(float(value), 'rad', 'deg'))
def onChkTimerChanged(self, state):
if state == Qt.Checked:
self.timer.start()
else:
self.timer.stop()
@Slot(str)
def addLinkToList(self, child_id):
if self.cboParent != None:
self.cboParent.addItem(child_id)
# --- Accessors
def childName(self):
if self.txtChild != None:
return self.txtChild.text()
else:
return ''
def parentName(self):
if self.cboParent != None:
return self.cboParent.currentText()
else:
return ''
def roll(self):
return self.spnRollRad.value()
def pitch(self):
return self.spnPitchRad.value()
def yaw(self):
return self.spnYawRad.value()
class LifeFrame(QFrame):
STATE_STOPPED = "stopped"
STATE_RUN = "running"
STATE_IDLE = "idle"
def __init__(self, parent=None):
super(LifeFrame, self).__init__(parent)
self._ui = Ui_life_frame()
self._motion = Rotate('/mobile_base/commands/velocity')
self._motion_thread = None
self._timer = QTimer()
#self._timer.setInterval(60000) #60s
self._timer.setInterval(250) #60s
QObject.connect(self._timer, SIGNAL('timeout()'), self, SLOT('update_progress_callback()'))
self._state = LifeFrame.STATE_STOPPED
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.angular_speed_spinbox.value())
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: life frame shutdown")
self._motion.shutdown()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def start(self):
self._state = LifeFrame.STATE_RUN
self._ui.run_progress.reset()
self._ui.idle_progress.reset()
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
def stop(self):
self._state = LifeFrame.STATE_STOPPED
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._timer.start()
self.start()
@Slot()
def on_stop_button_clicked(self):
self.stop()
self._timer.stop()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.angular_speed_spinbox.value())
##########################################################################
# Timer Callbacks
##########################################################################
@Slot()
def update_progress_callback(self):
if self._state == LifeFrame.STATE_RUN:
new_value = self._ui.run_progress.value()+1
if new_value == self._ui.run_progress.maximum():
print(" Switching to idle")
self._motion.stop()
self._state = LifeFrame.STATE_IDLE
else:
self._ui.run_progress.setValue(new_value)
if self._state == LifeFrame.STATE_IDLE:
new_value = self._ui.idle_progress.value()+1
if new_value == self._ui.idle_progress.maximum():
print(" Switching to run")
self.start()
else:
self._ui.idle_progress.setValue(new_value)
class ControllerManager(Plugin):
"""
Graphical frontend for managing ros_control controllers.
"""
_cm_update_freq = 1 # Hz
def __init__(self, context):
super(ControllerManager, self).__init__(context)
self.setObjectName('ControllerManager')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_controller_manager'),
'resource',
'controller_manager.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('ControllerManagerUi')
# Pop-up that displays controller information
self._popup_widget = QWidget()
ui_file = os.path.join(rp.get_path('rqt_controller_manager'),
'resource',
'controller_info.ui')
loadUi(ui_file, self._popup_widget)
self._popup_widget.setObjectName('ControllerInfoUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._cm_ns = [] # Namespace of the selected controller manager
self._controllers = [] # State of each controller
self._table_model = None
self._controller_lister = None
# Controller manager service proxies
self._load_srv = None
self._unload_srv = None
self._switch_srv = None
# Controller state icons
rospack = rospkg.RosPack()
path = rospack.get_path('rqt_controller_manager')
self._icons = {'running': QIcon(path + '/resource/led_green.png'),
'stopped': QIcon(path + '/resource/led_red.png'),
'uninitialized': QIcon(path + '/resource/led_off.png'),
'initialized': QIcon(path + '/resource/led_red.png')}
# Controllers display
table_view = self._widget.table_view
table_view.setContextMenuPolicy(Qt.CustomContextMenu)
table_view.customContextMenuRequested.connect(self._on_ctrl_menu)
table_view.doubleClicked.connect(self._on_ctrl_info)
header = table_view.horizontalHeader()
header.setSectionResizeMode(QHeaderView.ResizeToContents)
header.setContextMenuPolicy(Qt.CustomContextMenu)
header.customContextMenuRequested.connect(self._on_header_menu)
# Timer for controller manager updates
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._cm_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_ctrl_list_timer = QTimer(self)
self._update_ctrl_list_timer.setInterval(1000.0 /
self._cm_update_freq)
self._update_ctrl_list_timer.timeout.connect(self._update_controllers)
self._update_ctrl_list_timer.start()
# Signal connections
w = self._widget
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
def shutdown_plugin(self):
self._update_cm_list_timer.stop()
self._update_ctrl_list_timer.stop()
self._popup_widget.hide()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
update_combo(self._widget.cm_combo, self._list_cm())
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
# Setup services for communicating with the selected controller manager
self._set_cm_services(cm_ns)
# Controller lister for the selected controller manager
if cm_ns:
self._controller_lister = ControllerLister(cm_ns)
self._update_controllers()
else:
self._controller_lister = None
def _set_cm_services(self, cm_ns):
if cm_ns:
# NOTE: Persistent services are used for performance reasons.
# If the controller manager dies, we detect it and disconnect from
# it anyway
load_srv_name = _append_ns(cm_ns, 'load_controller')
self._load_srv = rospy.ServiceProxy(load_srv_name,
LoadController,
persistent=True)
unload_srv_name = _append_ns(cm_ns, 'unload_controller')
self._unload_srv = rospy.ServiceProxy(unload_srv_name,
UnloadController,
persistent=True)
switch_srv_name = _append_ns(cm_ns, 'switch_controller')
self._switch_srv = rospy.ServiceProxy(switch_srv_name,
SwitchController,
persistent=True)
else:
self._load_srv = None
self._unload_srv = None
self._switch_srv = None
def _update_controllers(self):
# Find controllers associated to the selected controller manager
controllers = self._list_controllers()
# Update controller display, if necessary
if self._controllers != controllers:
self._controllers = controllers
self._show_controllers() # NOTE: Model is recomputed from scratch
def _list_controllers(self):
"""
@return List of controllers associated to a controller manager
namespace. Contains both stopped/running controllers, as returned by
the C{list_controllers} service, plus uninitialized controllers with
configurations loaded in the parameter server.
@rtype [str]
"""
if not self._cm_ns:
return []
# Add loaded controllers first
controllers = self._controller_lister()
# Append potential controller configs found in the parameter server
all_ctrls_ns = _resolve_controllers_ns(self._cm_ns)
for name in get_rosparam_controller_names(all_ctrls_ns):
add_ctrl = not any(name == ctrl.name for ctrl in controllers)
if add_ctrl:
type_str = _rosparam_controller_type(all_ctrls_ns, name)
uninit_ctrl = ControllerState(name=name,
type=type_str,
state='uninitialized')
controllers.append(uninit_ctrl)
return controllers
def _show_controllers(self):
table_view = self._widget.table_view
self._table_model = ControllerTable(self._controllers, self._icons)
table_view.setModel(self._table_model)
def _on_ctrl_menu(self, pos):
# Get data of selected controller
row = self._widget.table_view.rowAt(pos.y())
if row < 0:
return # Cursor is not under a valid item
ctrl = self._controllers[row]
# Show context menu
menu = QMenu(self._widget.table_view)
if ctrl.state == 'running':
action_stop = menu.addAction(self._icons['stopped'], 'Stop')
action_kill = menu.addAction(self._icons['uninitialized'],
'Stop and Unload')
elif ctrl.state == 'stopped':
action_start = menu.addAction(self._icons['running'],
'Start again')
action_unload = menu.addAction(self._icons['uninitialized'],
'Unload')
elif ctrl.state == 'initialized':
action_start = menu.addAction(self._icons['running'], 'Start')
action_unload = menu.addAction(self._icons['uninitialized'],
'Unload')
elif ctrl.state == 'uninitialized':
action_load = menu.addAction(self._icons['stopped'], 'Load')
action_spawn = menu.addAction(self._icons['running'],
'Load and Start')
action = menu.exec_(self._widget.table_view.mapToGlobal(pos))
# Evaluate user action
if ctrl.state == 'running':
if action is action_stop:
self._stop_controller(ctrl.name)
elif action is action_kill:
self._stop_controller(ctrl.name)
self._unload_controller(ctrl.name)
elif ctrl.state == 'stopped' or ctrl.state == 'initialized':
if action is action_start:
self._start_controller(ctrl.name)
elif action is action_unload:
self._unload_controller(ctrl.name)
elif ctrl.state == 'uninitialized':
if action is action_load:
self._load_controller(ctrl.name)
if action is action_spawn:
self._load_controller(ctrl.name)
self._start_controller(ctrl.name)
def _on_ctrl_info(self, index):
popup = self._popup_widget
ctrl = self._controllers[index.row()]
popup.ctrl_name.setText(ctrl.name)
popup.ctrl_type.setText(ctrl.type)
res_model = QStandardItemModel()
model_root = QStandardItem('Claimed Resources')
res_model.appendRow(model_root)
for hw_res in ctrl.claimed_resources:
hw_iface_item = QStandardItem(hw_res.hardware_interface)
model_root.appendRow(hw_iface_item)
for res in hw_res.resources:
res_item = QStandardItem(res)
hw_iface_item.appendRow(res_item)
popup.resource_tree.setModel(res_model)
popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree))
popup.resource_tree.expandAll()
popup.move(QCursor.pos())
popup.show()
def _on_header_menu(self, pos):
header = self._widget.table_view.horizontalHeader()
# Show context menu
menu = QMenu(self._widget.table_view)
action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize')
action = menu.exec_(header.mapToGlobal(pos))
# Evaluate user action
if action is action_toggle_auto_resize:
if header.resizeMode(0) == QHeaderView.ResizeToContents:
header.setSectionResizeMode(QHeaderView.Interactive)
else:
header.setSectionResizeMode(QHeaderView.ResizeToContents)
def _load_controller(self, name):
self._load_srv.call(LoadControllerRequest(name=name))
def _unload_controller(self, name):
self._unload_srv.call(UnloadControllerRequest(name=name))
def _start_controller(self, name):
strict = SwitchControllerRequest.STRICT
req = SwitchControllerRequest(start_controllers=[name],
stop_controllers=[],
strictness=strict)
self._switch_srv.call(req)
def _stop_controller(self, name):
strict = SwitchControllerRequest.STRICT
req = SwitchControllerRequest(start_controllers=[],
stop_controllers=[name],
strictness=strict)
self._switch_srv.call(req)
class BagPlayerWidget(QWidget):
def __init__(self):
super(BagPlayerWidget, self).__init__()
rospkg_pack = rospkg.RosPack()
ui_file = os.path.join(rospkg_pack.get_path('bag_browser'), 'resource',
'BagPlayer.ui')
loadUi(ui_file, self)
self._play_icon = QIcon.fromTheme('media-playback-start')
self._pause_icon = QIcon.fromTheme('media-playback-pause')
self.qt_play_btn.setIcon(self._play_icon)
self._playing = False
self._current_msec = 0
self._max_msec = 1000000
self.qt_seekbar_slider.setTracking(False)
self._play_timer = QTimer()
self._play_timer.timeout.connect(self.on_image_update)
self._play_timer.setInterval(1) # [ms]
self._gui_timer = QTimer()
self._gui_timer.timeout.connect(self.on_gui_update)
self._gui_timer.setInterval(100) # [ms]
self._gui_timer.start()
def close(self):
self._play_timer.stop()
self._gui_timer.stop()
super(BagPlayerWidget, self).close()
def show(self, bagpath):
def converter(rosimg):
return QPixmap.fromImage(
BagPlayer.cvimg2qimg(BagPlayer.rosimg2cvimg(rosimg)))
self._bag_player = BagPlayer(bagpath, converter)
self.qt_filename_label.setText(os.path.basename(bagpath))
self._max_msec = self._bag_player.get_length_msec()
self.qt_seekbar_slider.setMinimum(0)
self.qt_seekbar_slider.setMaximum(self._max_msec - 1)
self.qt_seekbar_slider.setValue(0)
self.qt_time_numer_label.setText(str(0))
self.qt_time_denom_label.setText('/{0:.1f}[s]'.format(
int(self._max_msec * 0.001)))
self.image_widgets = {}
for i, name in enumerate(self._bag_player.get_names()):
self.image_widgets[name] = ImageViewWidget(title=name)
self.qt_imgs_gridlayout.addWidget(self.image_widgets[name], i / 2,
i % 2)
self.qt_imgs_gridlayout.setSpacing(3)
timestamps = self._bag_player.get_timestamps()
self._timestamps = [t - timestamps[0] for t in timestamps]
self._diff_msecs = [
a - b for a, b in zip(self._timestamps[1:], self._timestamps[:-1])
]
self.counter = 0
self.qt_seekbar_slider.setValue(0)
self._current_msec = 0
self.update_images(self.qt_seekbar_slider.value())
super(BagPlayerWidget, self).show()
def on_image_update(self):
if len(self._diff_msecs) > self.counter + 1:
self._current_msec = self._current_msec + self._diff_msecs[
self.counter]
self.counter += 1
else:
self._current_msec = 0
self.counter = 0
self._play_timer.setInterval(self._diff_msecs[self.counter])
self.update_images(self._current_msec)
def on_gui_update(self):
t = self._current_msec
self.qt_seekbar_slider.setValue(t)
self.qt_time_numer_label.setText('{0:.1f}'.format(t * 0.001))
def update_images(self, t):
imgs, names = self._bag_player.get_imgs(t)
for img, name in zip(imgs, names):
if img is None:
continue
self.image_widgets[name].set_image(img)
@Slot()
def on_qt_seekbar_slider_sliderPressed(self):
self._play_timer.stop()
self._gui_timer.stop()
self.qt_seekbar_slider.setTracking(True)
@Slot()
def on_qt_seekbar_slider_sliderReleased(self):
val = self.qt_seekbar_slider.value()
for i, ts in enumerate(self._timestamps):
if ts > val:
self._current_msec = self._timestamps[max(i - 1, 0)]
self.counter = i - 1
break
self.update_images(self._current_msec)
self.qt_seekbar_slider.setTracking(False)
if self._playing:
self._play_timer.start()
else:
self._play_timer.stop()
self._gui_timer.start()
@Slot()
def on_qt_play_btn_clicked(self):
if not self._playing:
self.qt_play_btn.setIcon(self._pause_icon)
self._play_timer.start()
else:
self.qt_play_btn.setIcon(self._play_icon)
self._play_timer.stop()
self._playing = not self._playing
class ROSData(object):
"""
Subscriber to ROS topic that buffers incoming data
"""
def __init__(self, topic_code, topic_item, start_time):
self.name = topic_code + '/' + topic_item
self.start_time = start_time
self.error = None
self.lock = threading.Lock()
self.buff_x = []
self.buff_y = []
self.interval = 100 # in milliseconds, period of regular update
self.timer = QTimer()
self.timer.setInterval(self.interval)
self.code = topic_code
self.item = topic_item
# go through options and decide what your self.data will be, given the ros subscribers
if topic_code == 's':
if topic_item == 'chi':
self.timer.timeout.connect(self.state_chi_cb)
elif topic_item == 'phi':
self.timer.timeout.connect(self.state_phi_cb)
elif topic_item == 'theta':
self.timer.timeout.connect(self.state_theta_cb)
elif topic_item == 'Va':
self.timer.timeout.connect(self.state_Va_cb)
elif topic_code == 'ci':
if topic_item == 'phi_c':
self.timer.timeout.connect(self.conin_phi_c_cb)
elif topic_item == 'theta_c':
self.timer.timeout.connect(self.conin_theta_c_cb)
elif topic_code == 'cc':
if topic_item == 'chi_c':
self.timer.timeout.connect(self.concom_chi_c_cb)
elif topic_item == 'Va_c':
self.timer.timeout.connect(self.concom_Va_c_cb)
self.timer.start()
def close(self):
self.timer.stop()
def state_chi_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(StateSub.chi)
def state_phi_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(StateSub.phi)
def state_theta_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(StateSub.theta)
def state_Va_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(StateSub.Va)
def conin_phi_c_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(ConInSub.phi_c)
def conin_theta_c_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(ConInSub.theta_c)
def concom_chi_c_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(ConComSub.chi_c)
def concom_Va_c_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(ConComSub.Va_c)
def next(self):
"""
Get the next data in the series
:returns: [xdata], [ydata]
"""
if self.error:
raise self.error
try:
self.lock.acquire()
buff_x = self.buff_x
buff_y = self.buff_y
self.buff_x = []
self.buff_y = []
finally:
self.lock.release()
return buff_x, buff_y
class Top(Plugin):
NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads']
OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ]
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ]
SORT_TYPE = [str, str, float, float, float ]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.iteritems():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex', int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class WorldMeshWorker(QObject):
finished = Signal() # class variable shared by all instances
def __init__(self,
finished_callback=None,
node_name='world_mesh_worker',
frame_id='local_origin',
marker_topic='world_mesh_marker',
refresh_ms=None,
new_node=False,
parent=None):
super(WorldMeshWorker, self).__init__(parent)
if new_node:
rospy.init_node(node_name, anonymous=True)
if finished_callback is not None:
self.finished.connect(finished_callback)
#TODO([email protected]) - how to shut down?
self.is_running = True
self.frame_id = frame_id
self.marker_pub = rospy.Publisher(marker_topic, Marker, queue_size=100)
self.file_resource = None
# if marker disappears, this will refresh it
# Rviz is only slow to load a mesh the first time
if refresh_ms is not None:
self.timer = QTimer()
self.timer.setInterval(refresh_ms) # in milliseconds
#self.timer.setTimerType(Qt.PreciseTimer)
self.timer.timeout.connect(self.on_timer_callback)
self.timer.start()
def publish_marker(self, file_resource):
self.file_resource = file_resource
marker = Marker()
marker.id = 0
marker.ns = 'world_mesh'
marker.type = marker.MESH_RESOURCE
marker.header.frame_id = self.frame_id
marker.action = marker.ADD
#marker.action = marker.DELETE
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
#marker.color.r = 1
#marker.color.g = 1
#marker.color.b = 1
#marker.color.a = 1
marker.lifetime = rospy.Duration(0)
marker.mesh_resource = file_resource
marker.mesh_use_embedded_materials = True
self.marker_pub.publish(marker)
def stop(self):
self.is_running = False
def on_timer_callback(self):
if self.file_resource is None:
return
self.publish_marker(self.file_resource)
class EnhancedLineEdit(QLineEdit):
stop_signal = Signal()
''' stop button was pressed '''
refresh_signal = Signal(str)
''' sends a refresh signal with current text. This signal is emmited if text was changed or button was pressed '''
def __init__(self, parent=None):
QLineEdit.__init__(self, parent=None)
self.process_active = False
# create a reload button with icon
self.button_reload = button_reload = QToolButton(self)
icon = QIcon.fromTheme("view-refresh",
nm.settings().icon('oxygen_view_refresh.png'))
button_reload.setIcon(icon)
button_reload.setCursor(Qt.ArrowCursor)
button_reload.setStyleSheet(
"QToolButton { border: none; padding: 0px; }")
# create a stop button with icon
self.button_stop = button_stop = QToolButton(self)
icon = QIcon.fromTheme("process-stop",
nm.settings().icon('oxygen_view_refresh.png'))
button_stop.setIcon(icon)
button_stop.setCursor(Qt.ArrowCursor)
button_stop.setStyleSheet(
"QToolButton { border: none; padding: 0px; }")
button_stop.hide()
# signals, clear lineEdit if btn pressed; change btn visibility on input
button_reload.clicked.connect(self._emit_refresh_text)
self.textChanged[str].connect(self.update_close_button)
button_stop.clicked.connect(self._process_stop)
frameWidth = self.style().pixelMetric(QStyle.PM_DefaultFrameWidth)
self.setStyleSheet("QLineEdit { padding-right: %dpx; } " %
(button_reload.sizeHint().width() + frameWidth + 1))
msz = self.minimumSizeHint()
self.setMinimumSize(
max(msz.width(),
button_reload.sizeHint().height() + frameWidth * 2 + 2),
max(msz.height(),
button_reload.sizeHint().height() + frameWidth * 2 + 2))
self._timer = QTimer(self)
self._timer.setSingleShot(True)
self._timer.setInterval(500)
self._timer.timeout.connect(self._emit_refresh_text)
def resizeEvent(self, event):
sz = self.button_reload.sizeHint()
frameWidth = self.style().pixelMetric(QStyle.PM_DefaultFrameWidth)
self.button_reload.move(self.rect().right() - frameWidth - sz.width(),
(self.rect().bottom() + 1 - sz.height()) / 2)
self.button_stop.move(self.rect().right() - frameWidth - sz.width(),
(self.rect().bottom() + 1 - sz.height()) / 2)
def update_close_button(self, text):
self._timer.stop()
self._timer.start()
# self.button_reload.setVisible(True if text else False)
def set_process_active(self, state):
if self.process_active != state:
self.process_active = state
self.button_reload.setVisible(not state)
self.button_stop.setVisible(state)
def _process_stop(self):
self.stop_signal.emit()
def _emit_refresh_text(self):
self.set_process_active(True)
self.refresh_signal.emit(self.text())
def keyPressEvent(self, event):
'''
Enable the ESC handling
'''
if event.key() == Qt.Key_Escape and self.text():
self.setText('')
self._timer.stop()
elif event.key() in [Qt.Key_Return, Qt.Key_Enter]:
self._timer.stop()
self._emit_refresh_text()
else:
event.accept()
QLineEdit.keyPressEvent(self, event)
class JointTrajectoryController(Plugin):
"""
Graphical frontend for a C{JointTrajectoryController}.
There are two modes for interacting with a controller:
1. B{Monitor mode} Joint displays are updated with the state reported
by the controller. This is a read-only mode and does I{not} send
control commands. Every time a new controller is selected, it starts
in monitor mode for safety reasons.
2. B{Control mode} Joint displays update the control command that is
sent to the controller. Commands are sent periodically evan if the
displays are not being updated by the user.
To control the aggressiveness of the motions, the maximum speed of the
sent commands can be scaled down using the C{Speed scaling control}
This plugin is able to detect and keep track of all active controller
managers, as well as the JointTrajectoryControllers that are I{running}
in each controller manager.
For a controller to be compatible with this plugin, it must comply with
the following requisites:
- The controller type contains the C{JointTrajectoryController}
substring, e.g., C{position_controllers/JointTrajectoryController}
- The controller exposes the C{command} and C{state} topics in its
ROS interface.
Additionally, there must be a URDF loaded with a valid joint limit
specification, namely position (when applicable) and velocity limits.
A reference implementation of the C{JointTrajectoryController} is
available in the C{joint_trajectory_controller} ROS package.
"""
_cmd_pub_freq = 10.0 # Hz
_widget_update_freq = 30.0 # Hz
_ctrlrs_update_freq = 1 # Hz
_min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration
jointStateChanged = Signal([dict])
_state_sub = None
def __init__(self, context):
super(JointTrajectoryController, self).__init__(context)
self.setObjectName('JointTrajectoryController')
# Initialize members
self._jtc_name = [] # Name of selected joint trajectory controller
self._cm_ns = [] # Namespace of the selected controller manager
self._joint_pos = {} # name->pos map for joints of selected controller
self._joint_names = [] # Ordered list of selected controller joints
self._jtc_joints_info = {} # Lazily evaluated as needed
self._robot_joint_limits = {} # Lazily evaluated on first use
self._node = context.node
self._widget = QWidget()
_, package_path = get_resource('packages',
'rqt_joint_trajectory_controller')
ui_file = os.path.join(package_path, 'share',
'rqt_joint_trajectory_controller', 'resource',
'joint_trajectory_controller.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JointTrajectoryControllerUi')
# Setup speed scaler
speed_scaling = DoubleEditor(1.0, 100.0)
speed_scaling.spin_box.setSuffix('%')
speed_scaling.spin_box.setValue(50.0)
speed_scaling.spin_box.setDecimals(0)
speed_scaling.setEnabled(False)
self._widget.speed_scaling_layout.addWidget(speed_scaling)
self._speed_scaling_widget = speed_scaling
speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
self._on_speed_scaling_change(speed_scaling.value())
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Timer for sending commands to active controller
self._update_cmd_timer = QTimer(self)
self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
self._update_cmd_timer.timeout.connect(self._update_cmd_cb)
# Timer for updating the joint widgets from the controller state
self._update_act_pos_timer = QTimer(self)
self._update_act_pos_timer.setInterval(1000.0 /
self._widget_update_freq)
self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)
# Timer for controller manager updates
self._list_cm = ['/controller_manager']
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_jtc_list_timer = QTimer(self)
self._update_jtc_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()
# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber
self._state_sub = self._node.create_subscription(
JointTrajectoryControllerState, '/state', self._state_cb, 10)
self._list_controllers = None
def shutdown_plugin(self):
self._update_cmd_timer.stop()
self._update_act_pos_timer.stop()
self._update_cm_list_timer.stop()
self._update_jtc_list_timer.stop()
self._unregister_state_sub()
self._unregister_cmd_pub()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
instance_settings.set_value('jtc_name', self._jtc_name)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
# Resore last session's controller, if running
self._update_jtc_list()
jtc_name = instance_settings.value('jtc_name')
jtc_combo = self._widget.jtc_combo
jtc_list = [
jtc_combo.itemText(i) for i in range(jtc_combo.count())
]
try:
idx = jtc_list.index(jtc_name)
jtc_combo.setCurrentIndex(idx)
except (ValueError):
pass
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
update_combo(self._widget.cm_combo, self._list_cm)
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if not self._list_controllers:
self._widget.jtc_combo.clear()
return
# List of running controllers with a valid joint limits specification
# for _all_ their joints
running_jtc = self._running_jtc_info()
if running_jtc and not self._robot_joint_limits:
self._robot_joint_limits = get_joint_limits(
n=self._node) # Lazy evaluation
valid_jtc = []
for jtc_info in running_jtc:
has_limits = all(
name in self._robot_joint_limits
for name in self._jtc_joint_names(jtc_name=jtc_info.name))
if has_limits:
valid_jtc.append(jtc_info)
valid_jtc_names = [data.name for data in valid_jtc]
# Update widget
update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))
def _on_speed_scaling_change(self, val):
self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
def _on_joint_state_change(self, actual_pos):
#assert(len(actual_pos) == len(self._joint_pos))
for name in actual_pos.keys():
try:
self._joint_pos[name]['position'] = actual_pos[name]
except (KeyError):
pass
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
if cm_ns:
self._list_controllers = list_controllers(self._node,
cm_ns).controller
# NOTE: Clear below is important, as different controller managers
# might have controllers with the same name but different
# configurations. Clearing forces controller re-discovery
self._widget.jtc_combo.clear()
self._update_jtc_list()
else:
self._list_controllers = None
def _on_jtc_change(self, jtc_name):
self._unload_jtc()
self._jtc_name = jtc_name
if self._jtc_name:
self._load_jtc()
def _on_jtc_enabled(self, val):
# Don't allow enabling if there are no controllers selected
if not self._jtc_name:
self._widget.enable_button.setChecked(False)
return
# Enable/disable joint displays
for joint_widget in self._joint_widgets():
joint_widget.setEnabled(val)
# Enable/disable speed scaling
self._speed_scaling_widget.setEnabled(val)
if val:
# Widgets send desired position commands to controller
self._update_act_pos_timer.stop()
self._update_cmd_timer.start()
else:
# Controller updates widgets with actual position
self._update_cmd_timer.stop()
self._update_act_pos_timer.start()
def _load_jtc(self):
# Initialize joint data corresponding to selected controller
running_jtc = self._running_jtc_info()
self._joint_names = next(
self._jtc_joint_names(x.name) for x in running_jtc
if x.name == self._jtc_name)
for name in self._joint_names:
self._joint_pos[name] = {}
# Update joint display
try:
layout = self._widget.joint_group.layout()
for name in self._joint_names:
limits = self._robot_joint_limits[name]
joint_widget = DoubleEditor(limits['min_position'],
limits['max_position'])
layout.addRow(name, joint_widget)
# NOTE: Using partial instead of a lambda because lambdas
# "will not evaluate/look up the argument values before it is
# effectively called, breaking situations like using a loop
# variable inside it"
from functools import partial
par = partial(self._update_single_cmd_cb, name=name)
joint_widget.valueChanged.connect(par)
except:
# TODO: Can we do better than swallow the exception?
from sys import exc_info
print('Unexpected error:', exc_info()[0])
# Enter monitor mode (sending commands disabled)
self._on_jtc_enabled(False)
# Setup ROS interfaces
jtc_ns = self._resolve_controller_ns(self._cm_ns, self._jtc_name)
state_topic = '/state'
cmd_topic = jtc_ns + '/joint_trajectory'
# self._state_sub = self._node.create_subscription(
# JointTrajectoryControllerState,
# '/state',
# self._state_cb,
# 10)
# self._state_sub
# print("state sub set up")
self._cmd_pub = self._node.create_publisher(JointTrajectory, cmd_topic,
1)
# Start updating the joint positions
self.jointStateChanged.connect(self._on_joint_state_change)
def _unload_jtc(self):
# Stop updating the joint positions
try:
self.jointStateChanged.disconnect(self._on_joint_state_change)
except:
pass
# Reset ROS interfaces
#self._unregister_state_sub()
self._unregister_cmd_pub()
# Clear joint widgets
# NOTE: Implementation is a workaround for:
# https://bugreports.qt-project.org/browse/QTBUG-15990 :(
layout = self._widget.joint_group.layout()
if layout is not None:
while layout.count():
layout.takeAt(0).widget().deleteLater()
# Delete existing layout by reparenting to temporary
QWidget().setLayout(layout)
self._widget.joint_group.setLayout(QFormLayout())
# Reset joint data
self._joint_names = []
self._joint_pos = {}
# Enforce monitor mode (sending commands disabled)
self._widget.enable_button.setChecked(False)
def _running_jtc_info(self):
controller_list = self._list_controllers
jtc_list = [
c for c in controller_list if 'JointTrajectoryController' in c.type
]
running_jtc_list = [c for c in jtc_list if c.state == 'active']
return running_jtc_list
def _unregister_cmd_pub(self):
if self._cmd_pub is not None:
self._node.destroy_publisher(self._cmd_pub)
self._state_sub = None
def _unregister_state_sub(self):
if self._state_sub is not None:
self._node.destroy_subscription(self._state_sub)
self._state_sub = None
def _state_cb(self, msg):
actual_pos = {}
for i in range(len(msg.joint_names)):
joint_name = msg.joint_names[i]
joint_pos = msg.actual.positions[i]
actual_pos[joint_name] = joint_pos
self.jointStateChanged.emit(actual_pos)
def _update_single_cmd_cb(self, val, name):
self._joint_pos[name]['command'] = val
def _update_cmd_cb(self):
dur = []
traj = JointTrajectory()
traj.joint_names = self._joint_names
point = JointTrajectoryPoint()
for name in traj.joint_names:
pos = self._joint_pos[name]['position']
cmd = pos
try:
cmd = self._joint_pos[name]['command']
except (KeyError):
pass
max_vel = self._robot_joint_limits[name]['max_velocity']
dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
point.positions.append(cmd)
point.time_from_start = Duration(seconds=(max(dur) /
self._speed_scale)).to_msg()
traj.points.append(point)
self._cmd_pub.publish(traj)
def _update_joint_widgets(self):
rclpy.spin_once(self._node)
joint_widgets = self._joint_widgets()
for id in range(len(joint_widgets)):
joint_name = self._joint_names[id]
try:
joint_pos = self._joint_pos[joint_name]['position']
joint_widgets[id].setValue(joint_pos)
except (KeyError):
pass # Can happen when first connected to controller
def _joint_widgets(self): # TODO: Cache instead of compute every time?
widgets = []
layout = self._widget.joint_group.layout()
for row_id in range(layout.rowCount()):
widgets.append(
layout.itemAt(row_id, QFormLayout.FieldRole).widget())
return widgets
def _jtc_joint_names(self, jtc_name):
# NOTE: We assume that there is at least one hardware interface that
# claims resources (there should be), and the resource list is fetched
# from the first available interface
if jtc_name not in self._jtc_joints_info:
self._jtc_joints_info[jtc_name] = call_get_parameters(
node=self._node,
node_name=jtc_name,
parameter_names=['joints']).values[0].string_array_value
return self._jtc_joints_info[jtc_name]
def _resolve_controller_ns(self, cm_ns, controller_name):
"""
Resolve a controller's namespace from that of the controller manager.
Controllers are assumed to live one level above the controller
manager, e.g.
>>> _resolve_controller_ns('/path/to/controller_manager', 'foo')
'/path/to/foo'
In the particular case in which the controller manager is not
namespaced, the controller is assumed to live in the root namespace
>>> _resolve_controller_ns('/', 'foo')
'/foo'
>>> _resolve_controller_ns('', 'foo')
'/foo'
@param cm_ns Controller manager namespace (can be an empty string)
@type cm_ns str
@param controller_name Controller name (non-empty string)
@type controller_name str
@return Controller namespace
@rtype str
"""
assert (controller_name)
ns = cm_ns.rsplit('/', 1)[0]
if ns != '/':
ns += '/'
ns += controller_name
return ns
class Conman(Plugin):
update_graph_sig = Signal(str)
def __init__(self, context):
super(Conman, self).__init__(context)
self._dotcode_sub = None
# Give QObjects reasonable names
self.setObjectName('Conman')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'rqt_conman.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('ConmanPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
palette = QPalette ()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#self._widget.subscribe_button.setCheckable(True)
self._widget.refresh_button.clicked[bool].connect(self._handle_refresh_clicked)
self._widget.commit_button.clicked[bool].connect(self._handle_commit_clicked)
#self._widget.xdot_widget.connect(
#self._widget.xdot_widget, SIGNAL('_update_graph'), self._widget.xdot_widget.set_dotcode)
self.update_graph_sig.connect(self._update_graph)
self.blocks = { }
self.groups = { }
self._ns = ""
self._actions_connected = False
self.enable_widgets(False)
self.new_dotcode_data = ''
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widgets)
#self.update_timer.start()
self._get_blocks = None
self._set_blocks = None
self._blocks_model = QStandardItemModel(0,4)
self._blocks_model.setHeaderData(0, Qt.Horizontal, "")
self._blocks_model.setHeaderData(1, Qt.Horizontal, "Action")
self._blocks_model.setHeaderData(2, Qt.Horizontal, "State")
self._blocks_model.setHeaderData(3, Qt.Horizontal, "Block")
self._widget.blocks_table.setModel(self._blocks_model)
self._blocks_delegate = BlocksDelegate(self)
self._widget.blocks_table.setItemDelegate(self._blocks_delegate)
self._blocks_model.itemChanged.connect(self.block_changed)
self._groups_model = QStandardItemModel(0,4)
self._groups_model.setHeaderData(0, Qt.Horizontal, "")
self._groups_model.setHeaderData(1, Qt.Horizontal, "")
self._groups_model.setHeaderData(2, Qt.Horizontal, "")
self._groups_model.setHeaderData(3, Qt.Horizontal, "Group")
self._widget.groups_table.setModel(self._groups_model)
self._groups_delegate = GroupsDelegate(self)
self._widget.groups_table.setItemDelegate(self._groups_delegate)
def block_changed(self, item):
row = item.row()
name = self._blocks_model.item(row,3).text()
block = self.blocks[name]
checked = item.checkState() == Qt.Checked
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
#def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def _get_result_cb(self, status, res):
rospy.loginfo("got result!")
self._blocks_model.setRowCount(0)
self._blocks_model.setRowCount(len(res.blocks))
for (i,block) in zip(range(len(res.blocks)),res.blocks):
# Store in dict
self.blocks[block.name] = block
cb = QStandardItem(True)
cb.setCheckable(True)
cb.setCheckState(Qt.Checked if block.state.value == conman_msgs.msg.TaskState.RUNNING else Qt.Unchecked)
cb.setTextAlignment(Qt.AlignHCenter)
cb.setTristate(True)
action = QStandardItem(True)
action.setText("")
state = QStandardItem(True)
state.setText(state_map[block.state.value])
name = QStandardItem(True)
name.setText(str(block.name))
self._blocks_model.setItem(i,0,cb)
self._blocks_model.setItem(i,1,action)
self._blocks_model.setItem(i,2,state)
self._blocks_model.setItem(i,3,name)
for (i,group) in zip(range(len(res.groups)),res.groups):
self.groups[group.name] = group
cb = QStandardItem(True)
cb.setCheckable(True)
cb.setCheckState(Qt.Checked)
cb.setTextAlignment(Qt.AlignHCenter)
cb.setEnabled(False)
name = QStandardItem(True)
name.setText(str(group.name))
self._groups_model.setItem(i,0,cb)
self._groups_model.setItem(i,3,name)
self._update_groups()
self._update_blocks()
self._widget.blocks_table.resizeColumnsToContents()
self._widget.blocks_table.horizontalHeader().setStretchLastSection(True)
self._widget.groups_table.resizeColumnsToContents()
self._widget.groups_table.horizontalHeader().setStretchLastSection(True)
def _update_blocks(self):
for (name, block) in self.blocks.items():
items = self._blocks_model.findItems(name, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
checked = self._blocks_model.item(row,0).checkState() == Qt.Checked
if checked and block.state.value != conman_msgs.msg.TaskState.RUNNING:
self._blocks_model.item(row,1).setText("ENABLE")
elif not checked and block.state.value == conman_msgs.msg.TaskState.RUNNING:
self._blocks_model.item(row,1).setText("DISABLE")
else:
self._blocks_model.item(row,1).setText("")
self._update_groups()
def _enable_group(self, index, enable):
name = self._groups_model.item(index, 3).text()
group = self.groups[name]
for member in group.members:
items = self._blocks_model.findItems(member, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
self._blocks_model.item(row,0).setCheckState(Qt.Checked if enable else Qt.Unchecked)
self._update_blocks()
def _update_groups(self):
for (name, group) in self.groups.items():
group_items = self._groups_model.findItems(name, column=3)
if len(group_items) > 0:
group_item = group_items[0]
else:
continue
group_row = group_item.row()
members_checked = []
for member in group.members:
items = self._blocks_model.findItems(member, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
members_checked.append(self._blocks_model.item(row,0).checkState() == Qt.Checked)
if all(members_checked):
check = Qt.Checked
else:
check = Qt.Unchecked
self._groups_model.item(group_row,0).setCheckState(check)
def _query_blocks(self):
if self._get_blocks and self._actions_connected:
if self._get_blocks.simple_state == actionlib.SimpleGoalState.DONE:
rospy.loginfo("Getting blocks...")
goal = conman_msgs.msg.GetBlocksGoal()
goal.publish_flow_graph = self._widget.generate_graph_checkbox.isChecked()
self._get_blocks.send_goal(goal, done_cb=self._get_result_cb)
def enable_widgets(self, enable):
#self._widget.generate_graph_checkbox.setEnabled(enable)
self._widget.force_enable_checkbox.setEnabled(enable)
#self._widget.disable_unused_button.setEnabled(enable)
self._widget.xdot_widget.setEnabled(enable)
self._widget.blocks_table.setEnabled(enable)
self._widget.groups_table.setEnabled(enable)
self._widget.regenerate_graph_button.setEnabled(enable)
def _handle_refresh_clicked(self, checked):
ns = self._widget.namespace_input.text()
if len(ns) > 0:
if self._ns != ns:
self._actions_connected = False
self._ns = ns
self.enable_widgets(False)
self._dotcode_sub = rospy.Subscriber(
ns+'/dotcode',
std_msgs.msg.String,
self._dotcode_msg_cb)
self._get_blocks = actionlib.SimpleActionClient(
ns+'/get_blocks_action',
conman_msgs.msg.GetBlocksAction)
self._set_blocks = actionlib.SimpleActionClient(
ns+'/set_blocks_action',
conman_msgs.msg.SetBlocksAction)
rospy.loginfo("Creating action clients on namespace '%s'..." % ns)
if not self._get_blocks.wait_for_server(rospy.Duration(2)):
rospy.loginfo("Timed out waiting for %s." % self._get_blocks.action_client.ns)
return
if not self._set_blocks.wait_for_server(rospy.Duration(2)):
rospy.loginfo("Timed out waiting for %s." % self._set_blocks.action_client.ns)
return
rospy.loginfo("Action clients created.")
self._actions_connected = True
self.enable_widgets(True)
self._query_blocks()
def _handle_commit_clicked(self, checked):
if self._set_blocks and self._actions_connected:
if self._get_blocks.simple_state == actionlib.SimpleGoalState.DONE:
rospy.loginfo("Setting blocks...")
goal = conman_msgs.msg.SetBlocksGoal()
goal.diff = True
goal.force = True
for i in range(self._blocks_model.rowCount()):
name = self._blocks_model.item(i,3).text()
action = self._blocks_model.item(i,1).text()
if action == 'DISABLE':
goal.disable.append(name)
elif action == 'ENABLE':
goal.enable.append(name)
self._set_blocks.send_goal(goal, done_cb=self._get_set_result_cb)
def _get_set_result_cb(self, status, res):
self._query_blocks()
@Slot(str)
def _update_graph(self,dotcode):
self._widget.xdot_widget.set_dotcode(dotcode, center=False)
def _dotcode_msg_cb(self, msg):
#self.new_dotcode_data = msg.data
self.update_graph_sig.emit(msg.data)
def _update_widgets(self):
self._update_groups()
self._update_blocks()
class BarrettDashboard(Plugin):
def __init__(self, context):
super(BarrettDashboard, self).__init__(context)
self._joint_sub = None
# Give QObjects reasonable names
self.setObjectName('BarrettDashboard')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'barrett_dashboard.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('BarrettDashboardPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.context = context
jp_widgets = [getattr(self._widget, 'jp_%d' % i) for i in range(7)]
jn_widgets = [getattr(self._widget, 'jn_%d' % i) for i in range(7)]
self.joint_widgets = zip(jp_widgets, jn_widgets)
tp_widgets = [getattr(self._widget, 'tp_%d' % i) for i in range(7)]
tn_widgets = [getattr(self._widget, 'tn_%d' % i) for i in range(7)]
self.torque_widgets = zip(tp_widgets, tn_widgets)
self.joint_signals = []
self.torque_signals = []
for (tp, tn) in self.torque_widgets:
tp.setRange(0.0, 1.0, False)
tn.setRange(1.0, 0.0, False)
tp.setValue(0.0)
tn.setValue(0.0)
# set random values for testing
#v = (2.0*random.random()) - 1.0
#tp.setValue(v if v >=0 else 0)
#tn.setValue(-v if v <0 else 0)
for (jp, jn) in self.joint_widgets:
jp.setRange(0.0, 1.0, False)
jn.setRange(1.0, 0.0, False)
jp.setValue(0.0)
jn.setValue(0.0)
# set random values for testing
#v = (2.0*random.random()) - 1.0
#jp.setValue(v if v >=0 else 0)
#jn.setValue(-v if v <0 else 0)
self.barrett_green = QColor(87, 186, 142)
self.barrett_green_dark = self.barrett_green.darker()
self.barrett_green_light = self.barrett_green.lighter()
self.barrett_blue = QColor(80, 148, 204)
self.barrett_blue_dark = self.barrett_blue.darker()
self.barrett_blue_light = self.barrett_blue.lighter()
self.barrett_red = QColor(232, 47, 47)
self.barrett_red_dark = self.barrett_red.darker()
self.barrett_red_light = self.barrett_red.lighter()
self.barrett_orange = QColor(255, 103, 43)
self.barrett_orange_dark = self.barrett_orange.darker()
#background_color = p.mid().color()
joint_bg_color = self.barrett_blue_dark.darker()
joint_fill_color = self.barrett_blue
joint_alarm_color = self.barrett_blue #self.barrett_blue_light
torque_bg_color = self.barrett_green_dark.darker()
torque_fill_color = self.barrett_green
torque_alarm_color = self.barrett_orange #self.barrett_green_light
temp_bg_color = self.barrett_red_dark.darker()
temp_fill_color = self.barrett_orange
temp_alarm_color = self.barrett_red
for w in jp_widgets + jn_widgets:
w.setAlarmLevel(0.80)
w.setFillColor(joint_fill_color)
w.setAlarmColor(joint_alarm_color)
p = w.palette()
p.setColor(tp.backgroundRole(), joint_bg_color)
w.setPalette(p)
for w in tp_widgets + tn_widgets:
w.setAlarmLevel(0.66)
w.setFillColor(torque_fill_color)
w.setAlarmColor(torque_alarm_color)
p = w.palette()
p.setColor(tp.backgroundRole(), torque_bg_color)
w.setPalette(p)
self._widget.hand_temp.setAlarmLevel(0.66)
self._widget.hand_temp.setFillColor(temp_fill_color)
self._widget.hand_temp.setAlarmColor(temp_alarm_color)
p = self._widget.hand_temp.palette()
p.setColor(self._widget.hand_temp.backgroundRole(), temp_bg_color)
self._widget.hand_temp.setPalette(p)
self._widget.hand_temp.setOrientation(Qt.Horizontal,
QwtThermo.RightScale)
self._widget.jf_0.setStyleSheet(
"QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }"
% (joint_bg_color.red(), joint_bg_color.green(),
joint_bg_color.blue(), joint_fill_color.red(),
joint_fill_color.green(), joint_fill_color.blue()))
self.urdf = rospy.get_param('robot_description')
self.robot = URDF()
self.robot = self.robot.from_xml_string(self.urdf)
self.pos_norm = [0] * 7
self.torque_norm = [0] * 7
self._joint_sub = rospy.Subscriber('joint_states',
sensor_msgs.msg.JointState,
self._joint_state_cb)
self._status_sub = rospy.Subscriber('status',
oro_barrett_msgs.msg.BarrettStatus,
self._status_cb)
self._hand_status_sub = rospy.Subscriber(
'hand/status', oro_barrett_msgs.msg.BHandStatus,
self._hand_status_cb)
self._update_status(-1)
self.safety_mode = -1
self.run_mode = 0
self.hand_run_mode = 0
self.hand_min_temperature = 40.0
self.hand_max_temperature = 65.0
self.hand_temperature = 0.0
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widget_values)
self.update_timer.start()
self.set_home_client = actionlib.SimpleActionClient(
'wam/set_home_action', oro_barrett_msgs.msg.SetHomeAction)
self.set_mode_client = actionlib.SimpleActionClient(
'wam/set_mode_action', oro_barrett_msgs.msg.SetModeAction)
self._widget.button_set_home.clicked[bool].connect(
self._handle_set_home_clicked)
self._widget.button_idle_wam.clicked[bool].connect(
self._handle_idle_wam_clicked)
self._widget.button_run_wam.clicked[bool].connect(
self._handle_run_wam_clicked)
self.bhand_init_client = actionlib.SimpleActionClient(
'hand/initialize_action', oro_barrett_msgs.msg.BHandInitAction)
self.bhand_set_mode_client = actionlib.SimpleActionClient(
'hand/set_mode_action', oro_barrett_msgs.msg.BHandSetModeAction)
self.grasp_client = actionlib.SimpleActionClient(
'grasp', BHandGraspAction)
self.release_client = actionlib.SimpleActionClient(
'release', BHandReleaseAction)
self.spread_client = actionlib.SimpleActionClient(
'spread', BHandSpreadAction)
self._widget.button_initialize_hand.clicked[bool].connect(
self._handle_initialize_hand_clicked)
self._widget.button_idle_hand.clicked[bool].connect(
self._handle_idle_hand_clicked)
self._widget.button_run_hand.clicked[bool].connect(
self._handle_run_hand_clicked)
self._widget.button_release_hand.clicked[bool].connect(
self._handle_release_hand_clicked)
self._widget.button_grasp_hand.clicked[bool].connect(
self._handle_grasp_hand_clicked)
self._widget.button_set_spread.clicked[bool].connect(
self._handle_spread_hand_clicked)
self._widget.resizeEvent = self._handle_resize
def _handle_resize(self, event):
for i, ((w1, w2), (w3, w4)) in enumerate(
zip(self.joint_widgets, self.torque_widgets)):
width = 0.8 * getattr(self._widget,
'jcc_%d' % i).contentsRect().width()
w1.setPipeWidth(width)
w2.setPipeWidth(width)
w3.setPipeWidth(width)
w4.setPipeWidth(width)
def _handle_set_home_clicked(self, checked):
if checked:
self.set_home()
def _update_status(self, status):
if status == -1:
self._widget.safety_mode.setText('UNKNOWN SAFETY MODE')
self._widget.safety_mode.setToolTip(
'The WAM is in an unknown state. Proceed with caution.')
color = QColor(200, 200, 200)
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
elif status == 0:
self._widget.safety_mode.setText('E-STOP')
self._widget.safety_mode.setToolTip(
'The WAM is stopped and unpowered. Determine the source of the fault and reset the arm to continue using it.'
)
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_red
else:
if not self.homed:
self._widget.safety_mode.setText('UNCALIBRATED')
self._widget.safety_mode.setToolTip(
'The WAM is not calibrated. Please place it in the calibration posture and click the "Calibrate" button.'
)
self._widget.button_set_home.setEnabled(True)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_orange
else:
if status == 1:
self._widget.safety_mode.setText('IDLE')
self._widget.safety_mode.setToolTip(
'The WAM is running but all joints are passive. It is safe to home the arm.'
)
self._widget.button_set_home.setEnabled(True)
self._widget.button_idle_wam.setEnabled(True)
self._widget.button_run_wam.setEnabled(True)
color = self.barrett_blue
elif status == 2:
self._widget.safety_mode.setText('ACTIVE')
self._widget.safety_mode.setToolTip(
'The WAM is running and all joints are active. Proceed with caution.'
)
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_green
darker = color.darker()
lighter = color.lighter()
self._widget.safety_mode.setStyleSheet(
"QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }"
% (color.red(), color.green(), color.blue(), lighter.red(),
lighter.green(), lighter.blue()))
def _update_widget_values(self):
if self.safety_mode in (oro_barrett_msgs.msg.SafetyMode.ACTIVE,
oro_barrett_msgs.msg.SafetyMode.IDLE):
for (v, (tp, tn)) in zip(self.torque_norm, self.torque_widgets):
tp.setEnabled(True)
tn.setEnabled(True)
tp.setValue(v if v >= 0 else 0)
tn.setValue(-v if v < 0 else 0)
for (v, (jp, jn)) in zip(self.pos_norm, self.joint_widgets):
jp.setEnabled(True)
jn.setEnabled(True)
jp.setValue(v if v >= 0 else 0)
jn.setValue(-v if v < 0 else 0)
else:
for (p, n) in self.joint_widgets + self.torque_widgets:
p.setEnabled(True)
n.setEnabled(True)
self._widget.hand_temp.setValue(
(self.hand_temperature - self.hand_min_temperature) /
(self.hand_max_temperature - self.hand_min_temperature))
self._update_status(self.safety_mode)
self._update_buttons(self.run_mode, self.hand_run_mode)
def _update_buttons(self, run_mode, hand_run_mode):
if run_mode == oro_barrett_msgs.msg.RunMode.IDLE:
self._widget.button_idle_wam.setChecked(True)
self._widget.button_run_wam.setChecked(False)
else:
self._widget.button_idle_wam.setChecked(False)
self._widget.button_run_wam.setChecked(True)
if hand_run_mode == oro_barrett_msgs.msg.RunMode.RUN:
self._widget.button_idle_hand.setChecked(False)
self._widget.button_run_hand.setChecked(True)
else:
self._widget.button_idle_hand.setChecked(True)
self._widget.button_run_hand.setChecked(False)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
#def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def _joint_state_cb(self, msg):
joint_pos_map = dict(zip(msg.name, msg.position))
for (name, pos, torque, i) in zip(msg.name, msg.position, msg.effort,
range(7)):
joint = self.robot.joint_map[name]
self.pos_norm[i] = 2.0 * (
(pos - joint.limit.lower) /
(joint.limit.upper - joint.limit.lower)) - 1.0
self.torque_norm[i] = torque / joint.limit.effort
def _status_cb(self, msg):
self.safety_mode = msg.safety_mode.value
self.run_mode = msg.run_mode.value
self.homed = msg.homed
if self.safety_mode == oro_barrett_msgs.msg.SafetyMode.ACTIVE:
self._widget.button_initialize_hand.setEnabled(False)
elif self.safety_mode == oro_barrett_msgs.msg.SafetyMode.ESTOP:
self._widget.button_initialize_hand.setEnabled(False)
self._widget.button_idle_hand.setEnabled(False)
self._widget.button_run_hand.setEnabled(False)
def _hand_status_cb(self, msg):
self.hand_initialized = msg.initialized
self.hand_temperature = max(msg.temperature)
self.hand_run_mode = msg.run_mode.value
def _handle_set_home_clicked(self, checked):
goal = oro_barrett_msgs.msg.SetHomeGoal()
self.set_home_client.send_goal(goal)
def _handle_idle_wam_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.SetModeGoal()
goal.mode.value = oro_barrett_msgs.msg.RunMode.IDLE
self.set_mode_client.send_goal(goal)
def _handle_run_wam_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.SetModeGoal()
goal.mode.value = oro_barrett_msgs.msg.RunMode.RUN
self.set_mode_client.send_goal(goal)
def _handle_initialize_hand_clicked(self, checked):
if self.safety_mode == oro_barrett_msgs.msg.SafetyMode.IDLE:
goal = oro_barrett_msgs.msg.BHandInitGoal()
self.bhand_init_client.send_goal(goal)
def _handle_run_hand_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.BHandSetModeGoal()
goal.run_mode.value = oro_barrett_msgs.msg.RunMode.RUN
self.bhand_set_mode_client.send_goal(goal)
def _handle_idle_hand_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.BHandSetModeGoal()
goal.run_mode.value = oro_barrett_msgs.msg.RunMode.IDLE
self.bhand_set_mode_client.send_goal(goal)
def _get_grasp_mask(self):
return [
self._widget.button_use_f1.isChecked(),
self._widget.button_use_f2.isChecked(),
self._widget.button_use_f3.isChecked()
]
def _handle_release_hand_clicked(self, checked):
goal = BHandReleaseGoal()
goal.release_mask = self._get_grasp_mask()
goal.release_speed = [3.0, 3.0, 3.0]
self.release_client.send_goal(goal)
def _handle_grasp_hand_clicked(self, checked):
goal = BHandGraspGoal()
goal.grasp_mask = self._get_grasp_mask()
goal.grasp_speed = [1.0, 1.0, 1.0]
goal.grasp_effort = [1.0, 1.0, 1.0]
goal.min_fingertip_radius = 0.0
self.grasp_client.send_goal(goal)
def _handle_spread_hand_clicked(self, checked):
goal = BHandSpreadGoal()
goal.spread_position = self._widget.spread_slider.value(
) / 1000.0 * 3.141
self.spread_client.send_goal(goal)
class DynamicTimeline(QGraphicsScene):
"""
BagTimeline contains bag files, all information required to display the bag data visualization on the screen
Also handles events
"""
status_bar_changed_signal = Signal()
selected_region_changed = Signal(rospy.Time, rospy.Time)
timeline_updated = Signal()
Topic = collections.namedtuple('Topic', ['subscriber', 'queue'])
Message = collections.namedtuple('Message', ['stamp', 'message'])
def __init__(self, context, publish_clock):
"""
:param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext''
"""
super(DynamicTimeline, self).__init__()
# key is topic name, value is a named tuple of type Topic. The deque
# contains named tuples of type Message
self._topics = {}
# key is the data type, value is a list of topics with that type
self._datatypes = {}
self._topic_lock = threading.RLock()
self.background_task = None # Display string
self.background_task_cancel = False
# Playing / Recording
self._playhead_lock = threading.RLock()
self._max_play_speed = 1024.0 # fastest X play speed
self._min_play_speed = 1.0 / 1024.0 # slowest X play speed
self._play_speed = 0.0
self._play_all = False
self._playhead_positions_cvs = {}
self._playhead_positions = {} # topic -> position
self._message_loaders = {}
self._messages_cvs = {}
self._messages = {} # topic -> msg_data
self._message_listener_threads = {
} # listener -> MessageListenerThread
self._player = False
self._publish_clock = publish_clock
self._recorder = None
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
self.wrap = True # should the playhead wrap when it reaches the end?
self.stick_to_end = False # should the playhead stick to the end?
self._play_timer = QTimer()
self._play_timer.timeout.connect(self.on_idle)
self._play_timer.setInterval(3)
self._redraw_timer = None # timer which can be used to periodically redraw the timeline
# Plugin popup management
self._context = context
self.popups = {}
self._views = []
self._listeners = {}
# Initialize scene
# the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast.
# otherwise a dark qt theme will default it to black and the frame render pen will be unreadable
self.setBackgroundBrush(Qt.white)
self._timeline_frame = DynamicTimelineFrame(self)
self._timeline_frame.setPos(0, 0)
self.addItem(self._timeline_frame)
self.background_progress = 0
self.__closed = False
# timer to periodically redraw the timeline every so often
self._start_redraw_timer()
def get_context(self):
"""
:returns: the ROS_GUI context, 'PluginContext'
"""
return self._context
def _start_redraw_timer(self):
if not self._redraw_timer:
self._redraw_timer = rospy.Timer(rospy.Duration(0.5),
self._redraw_timeline)
def _stop_redraw_timer(self):
if self._redraw_timer:
self._redraw_timer.shutdown()
self._redraw_timer = None
def handle_close(self):
"""
Cleans up the timeline, subscribers and any threads
"""
if self.__closed:
return
else:
self.__closed = True
self._play_timer.stop()
for topic in self._get_topics():
self.stop_publishing(topic)
self._message_loaders[topic].stop()
if self._player:
self._player.stop()
if self._recorder:
self._recorder.stop()
if self.background_task is not None:
self.background_task_cancel = True
self._timeline_frame.handle_close()
for topic in self._topics:
self._topics[topic][0].unregister() # unregister the subscriber
for frame in self._views:
if frame.parent():
self._context.remove_widget(frame)
def _redraw_timeline(self, timer):
# save the playhead so that the redraw doesn't move it
playhead = self._timeline_frame._playhead
if playhead is None:
start = self._timeline_frame.play_region[0] is None
end = self._timeline_frame.play_region[1] is None
else:
start = True if playhead <= self._timeline_frame.play_region[
0] else False
end = True if playhead >= self._timeline_frame.play_region[
1] else False
# do not keep setting this if you want the timeline to just grow.
self._timeline_frame._start_stamp = self._get_start_stamp()
self._timeline_frame._end_stamp = self._get_end_stamp()
self._timeline_frame.reset_zoom()
if end:
# use play region instead of time.now to stop playhead going past
# the end of the region, which causes problems with
# navigate_previous
self._timeline_frame._set_playhead(
self._timeline_frame.play_region[1])
elif start:
self._timeline_frame._set_playhead(
self._timeline_frame.play_region[0])
else:
if playhead:
self._timeline_frame._set_playhead(playhead)
self.timeline_updated.emit()
def topic_callback(self, msg, topic):
"""
Called whenever a message is received on any of the subscribed topics
:param topic: the topic on which the message was received
:param msg: the message received
"""
message = self.Message(stamp=rospy.Time.now(), message=msg)
with self._topic_lock:
self._topics[topic].queue.append(message)
# Invalidate entire cache for this topic
with self._timeline_frame.index_cache_cv:
self._timeline_frame.invalidated_caches.add(topic)
# if topic in self._timeline_frame.index_cache:
# del self._timeline_frame.index_cache[topic]
self._timeline_frame.index_cache_cv.notify()
def add_topic(self, topic, type, num_msgs=20):
"""Creates an indexing thread for the new topic. Fixes the borders and notifies
the indexing thread to index the new items bags
:param topic: a topic to listen to
:param type: type of the topic to listen to
:param num_msgs: number of messages to retain on this topic. If this
value is exceeded, the oldest messages will be dropped
:return: false if topic already in the timeline, true otherwise
"""
# first item in each sub-list is the name, second is type
if topic not in self._topics:
self._topics[topic] = self.Topic(
subscriber=rospy.Subscriber(topic,
type,
queue_size=1,
callback=self.topic_callback,
callback_args=topic),
queue=collections.deque(maxlen=num_msgs))
self._datatypes.setdefault(type, []).append(topic)
else:
return False
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
self._timeline_frame._start_stamp = self._get_start_stamp()
self._timeline_frame._end_stamp = self._get_end_stamp()
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
)
# If this is the first bag, reset the timeline
self._timeline_frame.reset_zoom()
# Invalidate entire cache for this topic
with self._timeline_frame.index_cache_cv:
self._timeline_frame.invalidated_caches.add(topic)
if topic in self._timeline_frame.index_cache:
del self._timeline_frame.index_cache[topic]
self._timeline_frame.index_cache_cv.notify()
return True
# TODO Rethink API and if these need to be visible
def _get_start_stamp(self):
"""
:return: stamp of the first message received on any of the topics, or none if no messages have been received, ''rospy.Time''
"""
with self._topic_lock:
start_stamp = None
for unused_topic_name, topic_tuple in self._topics.items():
topic_start_stamp = topic_helper.get_start_stamp(topic_tuple)
if topic_start_stamp is not None and (
start_stamp is None
or topic_start_stamp < start_stamp):
start_stamp = topic_start_stamp
return start_stamp
def _get_end_stamp(self):
"""
:return: stamp of the last message received on any of the topics, or none if no messages have been received, ''rospy.Time''
"""
with self._topic_lock:
end_stamp = None
for unused_topic_name, topic_tuple in self._topics.items():
topic_end_stamp = topic_helper.get_end_stamp(topic_tuple)
if topic_end_stamp is not None and (
end_stamp is None or topic_end_stamp > end_stamp):
end_stamp = topic_end_stamp
return end_stamp
def _get_topics(self):
"""
:return: sorted list of topic names, ''list(str)''
"""
with self._topic_lock:
topics = []
for topic in self._topics:
topics.append(topic)
return sorted(topics)
def _get_topics_by_datatype(self):
"""
:return: dict of list of topics for each datatype, ''dict(datatype:list(topic))''
"""
with self._topic_lock:
return self._datatypes
def get_datatype(self, topic):
"""
:return: datatype associated with a topic, ''str''
:raises: if there are multiple datatypes assigned to a single topic, ''Exception''
"""
with self._topic_lock:
topic_types = []
for datatype in self._datatypes:
if topic in self._datatypes[datatype]:
if len(topic_types) == 1:
raise Exception(
"Topic {0} had multiple datatypes ({1}) associated with it"
.format(topic, str(topic_types)))
topic_types.append(datatype._type)
if not topic_types:
return None
else:
return topic_types[0]
def get_entries(self, topics, start_stamp, end_stamp):
"""
generator function for topic entries
:param topics: list of topics to query, ''list(str)''
:param start_stamp: stamp to start at, ''rospy.Time''
:param end_stamp: stamp to end at, ''rospy,Time''
:returns: messages on the given topics in chronological order, ''msg''
"""
with self._topic_lock:
topic_entries = []
# make sure that we can handle a single topic as well
for topic in topics:
if not topic in self._topics:
rospy.logwarn(
"Dynamic Timeline : Topic {0} was not in the topic list. Skipping."
.format(topic))
continue
# don't bother with topics if they don't overlap the requested time range
topic_start_time = topic_helper.get_start_stamp(
self._topics[topic])
if topic_start_time is not None and topic_start_time > end_stamp:
continue
topic_end_time = topic_helper.get_end_stamp(
self._topics[topic])
if topic_end_time is not None and topic_end_time < start_stamp:
continue
topic_queue = self._topics[topic].queue
start_ind, first_entry = self._entry_at(
start_stamp, topic_queue)
# entry returned might be the latest one before the start stamp
# if there isn't one exactly on the stamp - if so, start from
# the next entry
if first_entry.stamp < start_stamp:
start_ind += 1
# entry at always returns entry at or before the given stamp, so
# no manipulation needed.
end_ind, last_entry = self._entry_at(end_stamp, topic_queue)
topic_entries.extend(
list(itertools.islice(topic_queue, start_ind,
end_ind + 1)))
for entry in sorted(topic_entries, key=lambda x: x.stamp):
yield entry
def get_entries_with_bags(self, topic, start_stamp, end_stamp):
"""
generator function for bag entries
:param topics: list of topics to query, ''list(str)''
:param start_stamp: stamp to start at, ''rospy.Time''
:param end_stamp: stamp to end at, ''rospy,Time''
:returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
from rosbag import bag # for _mergesort
bag_entries = []
bag_by_iter = {}
for b in self._bags:
bag_start_time = bag_helper.get_start_stamp(b)
if bag_start_time is not None and bag_start_time > end_stamp:
continue
bag_end_time = bag_helper.get_end_stamp(b)
if bag_end_time is not None and bag_end_time < start_stamp:
continue
connections = list(b._get_connections(topic))
it = iter(b._get_entries(connections, start_stamp, end_stamp))
bag_by_iter[it] = b
bag_entries.append(it)
for entry, it in bag._mergesort(bag_entries,
key=lambda entry: entry.time):
yield bag_by_iter[it], entry
def _entry_at(self, t, queue):
"""Get the entry and index in the queue at the given time.
:param ``rospy.Time`` t: time to check
:param ``collections.deque`` queue: deque to look at
:return: (index, Message) tuple. If there is no message in the queue at
the exact time, the previous index is returned. If the time is
before or after the first and last message times, the first or last
index and message are returned
"""
# Gives the index to insert into to retain a sorted queue. The topic queues
# should always be sorted due to time passing.
# ind = bisect.bisect(queue, self.Message(stamp=t, message=''))
# Can't use bisect here in python3 unless the correct operators
# are defined for sorting, so do it manually
try:
ind = next(i for i, msg in enumerate(queue) if t < msg.stamp)
except StopIteration:
ind = len(queue)
# first or last indices
if ind == len(queue):
return (ind - 1, queue[-1])
elif ind == 0:
return (0, queue[0])
# non-end indices
cur = queue[ind]
if cur.stamp == t:
return (ind, cur)
else:
return (ind - 1, queue[ind - 1])
def get_entry(self, t, topic):
"""Get a message in the queues for a specific topic
:param ``rospy.Time`` t: time of the message to retrieve
:param str topic: the topic to be accessed
:return: message corresponding to time t and topic. If there is no
corresponding entry at exactly the given time, the latest entry
before the given time is returned. If the topic does not exist, or
there is no entry, None.
"""
with self._topic_lock:
entry = None
if topic in self._topics:
_, entry = self._entry_at(t, self._topics[topic].queue)
return entry
def get_entry_before(self, t):
"""
Get the latest message before the given time on any of the topics
:param t: time, ``rospy.Time``
:return: tuple of (topic, entry) corresponding to time t, ``(str, msg)``
"""
with self._topic_lock:
entry_topic, entry = None, None
for topic in self._topics:
_, topic_entry = self._entry_at(t - rospy.Duration(0, 1),
self._topics[topic].queue)
if topic_entry and (not entry
or topic_entry.stamp > entry.stamp):
entry_topic, entry = topic, topic_entry
return entry_topic, entry
def get_entry_after(self, t):
"""
Get the earliest message on any topic after the given time
:param t: time, ''rospy.Time''
:return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)''
"""
with self._topic_lock:
entry_topic, entry = None, None
for topic in self._topics:
ind, _ = self._entry_at(t, self._topics[topic].queue)
# ind is the index of the entry at (if it exists) or before time
# t - we want the one after this. Make sure that the given index
# isn't out of bounds
ind = ind + 1 if ind + 1 < len(
self._topics[topic].queue) else ind
topic_entry = self._topics[topic].queue[ind]
if topic_entry and (not entry
or topic_entry.stamp < entry.stamp):
entry_topic, entry = topic, topic_entry
return entry_topic, entry
def get_next_message_time(self):
"""
:return: time of the message after the current playhead position,''rospy.Time''
"""
if self._timeline_frame.playhead is None:
return None
_, entry = self.get_entry_after(self._timeline_frame.playhead)
if entry is None:
return self._timeline_frame._end_stamp
return entry.stamp
def get_previous_message_time(self):
"""
:return: time of the message before the current playhead position,''rospy.Time''
"""
if self._timeline_frame.playhead is None:
return None
_, entry = self.get_entry_before(self._timeline_frame.playhead)
if entry is None:
return self._timeline_frame._start_stamp
return entry.stamp
def resume(self):
if (self._player):
self._player.resume()
### Copy messages to...
def start_background_task(self, background_task):
"""
Verify that a background task is not currently running before starting a new one
:param background_task: name of the background task, ''str''
"""
if self.background_task is not None:
QMessageBox(
QMessageBox.Warning, 'Exclamation',
'Background operation already running:\n\n%s' %
self.background_task, QMessageBox.Ok).exec_()
return False
self.background_task = background_task
self.background_task_cancel = False
return True
def stop_background_task(self):
self.background_task = None
def copy_region_to_bag(self, filename):
if len(self._bags) > 0:
self._export_region(filename, self._timeline_frame.topics,
self._timeline_frame.play_region[0],
self._timeline_frame.play_region[1])
def _export_region(self, path, topics, start_stamp, end_stamp):
"""
Starts a thread to save the current selection to a new bag file
:param path: filesystem path to write to, ''str''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
if not self.start_background_task('Copying messages to "%s"' % path):
return
# TODO implement a status bar area with information on the current save status
bag_entries = list(
self.get_entries_with_bags(topics, start_stamp, end_stamp))
if self.background_task_cancel:
return
# Get the total number of messages to copy
total_messages = len(bag_entries)
# If no messages, prompt the user and return
if total_messages == 0:
QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found',
QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Open the path for writing
try:
export_bag = rosbag.Bag(path, 'w')
except Exception:
QMessageBox(QMessageBox.Warning, 'rqt_bag',
'Error opening bag file [%s] for writing' % path,
QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Run copying in a background thread
self._export_thread = threading.Thread(target=self._run_export_region,
args=(export_bag, topics,
start_stamp, end_stamp,
bag_entries))
self._export_thread.start()
def _run_export_region(self, export_bag, topics, start_stamp, end_stamp,
bag_entries):
"""
Threaded function that saves the current selection to a new bag file
:param export_bag: bagfile to write to, ''rosbag.bag''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
total_messages = len(bag_entries)
update_step = max(1, total_messages / 100)
message_num = 1
progress = 0
# Write out the messages
for bag, entry in bag_entries:
if self.background_task_cancel:
break
try:
topic, msg, t = self.read_message(bag, entry.position)
export_bag.write(topic, msg, t)
except Exception as ex:
qWarning('Error exporting message at position %s: %s' %
(str(entry.position), str(ex)))
export_bag.close()
self.stop_background_task()
return
if message_num % update_step == 0 or message_num == total_messages:
new_progress = int(100.0 *
(float(message_num) / total_messages))
if new_progress != progress:
progress = new_progress
if not self.background_task_cancel:
self.background_progress = progress
self.status_bar_changed_signal.emit()
message_num += 1
# Close the bag
try:
self.background_progress = 0
self.status_bar_changed_signal.emit()
export_bag.close()
except Exception as ex:
QMessageBox(
QMessageBox.Warning, 'rqt_bag',
'Error closing bag file [%s]: %s' %
(export_bag.filename, str(ex)), QMessageBox.Ok).exec_()
self.stop_background_task()
def read_message(self, topic, position):
with self._topic_lock:
return self.get_entry(position, topic).message
def on_mouse_down(self, event):
"""
When the user clicks down in the timeline.
"""
if event.buttons() == Qt.LeftButton:
self._timeline_frame.on_left_down(event)
elif event.buttons() == Qt.MidButton:
self._timeline_frame.on_middle_down(event)
elif event.buttons() == Qt.RightButton:
topic = self._timeline_frame.map_y_to_topic(event.y())
TimelinePopupMenu(self, event, topic)
def on_mouse_up(self, event):
self._timeline_frame.on_mouse_up(event)
def on_mouse_move(self, event):
self._timeline_frame.on_mouse_move(event)
def on_mousewheel(self, event):
self._timeline_frame.on_mousewheel(event)
# Zooming
def zoom_in(self):
self._timeline_frame.zoom_in()
def zoom_out(self):
self._timeline_frame.zoom_out()
def reset_zoom(self):
self._timeline_frame.reset_zoom()
def translate_timeline_left(self):
self._timeline_frame.translate_timeline_left()
def translate_timeline_right(self):
self._timeline_frame.translate_timeline_right()
### Publishing
def is_publishing(self, topic):
return self._player and self._player.is_publishing(topic)
def start_publishing(self, topic):
if not self._player and not self._create_player():
return False
self._player.start_publishing(topic)
return True
def stop_publishing(self, topic):
if not self._player:
return False
self._player.stop_publishing(topic)
return True
def _create_player(self):
if not self._player:
try:
self._player = Player(self)
if self._publish_clock:
self._player.start_clock_publishing()
except Exception as ex:
qWarning('Error starting player; aborting publish: %s' %
str(ex))
return False
return True
def set_publishing_state(self, start_publishing):
if start_publishing:
for topic in self._timeline_frame.topics:
if not self.start_publishing(topic):
break
else:
for topic in self._timeline_frame.topics:
self.stop_publishing(topic)
# property: play_all
def _get_play_all(self):
return self._play_all
def _set_play_all(self, play_all):
if play_all == self._play_all:
return
self._play_all = not self._play_all
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
play_all = property(_get_play_all, _set_play_all)
def toggle_play_all(self):
self.play_all = not self.play_all
### Playing
def on_idle(self):
self._step_playhead()
def _step_playhead(self):
"""
moves the playhead to the next position based on the desired position
"""
# Reset when the playing mode switchs
if self._timeline_frame.playhead != self.last_playhead:
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
if self._play_all:
self.step_next_message()
else:
self.step_fixed()
def step_fixed(self):
"""
Moves the playhead a fixed distance into the future based on the current play speed
"""
if self.play_speed == 0.0 or not self._timeline_frame.playhead:
self.last_frame = None
self.last_playhead = None
return
now = rospy.Time.from_sec(time.time())
if self.last_frame:
# Get new playhead
if self.stick_to_end:
new_playhead = self.end_stamp
else:
new_playhead = self._timeline_frame.playhead + rospy.Duration.from_sec(
(now - self.last_frame).to_sec() * self.play_speed)
start_stamp, end_stamp = self._timeline_frame.play_region
if new_playhead > end_stamp:
if self.wrap:
if self.play_speed > 0.0:
new_playhead = start_stamp
else:
new_playhead = end_stamp
else:
new_playhead = end_stamp
if self.play_speed > 0.0:
self.stick_to_end = True
elif new_playhead < start_stamp:
if self.wrap:
if self.play_speed < 0.0:
new_playhead = end_stamp
else:
new_playhead = start_stamp
else:
new_playhead = start_stamp
# Update the playhead
self._timeline_frame.playhead = new_playhead
self.last_frame = now
self.last_playhead = self._timeline_frame.playhead
def step_next_message(self):
"""
Move the playhead to the next message
"""
if self.play_speed <= 0.0 or not self._timeline_frame.playhead:
self.last_frame = None
self.last_playhead = None
return
if self.last_frame:
if not self.desired_playhead:
self.desired_playhead = self._timeline_frame.playhead
else:
delta = rospy.Time.from_sec(time.time()) - self.last_frame
if delta > rospy.Duration.from_sec(0.1):
delta = rospy.Duration.from_sec(0.1)
self.desired_playhead += delta
# Get the occurrence of the next message
next_message_time = self.get_next_message_time()
if next_message_time < self.desired_playhead:
self._timeline_frame.playhead = next_message_time
else:
self._timeline_frame.playhead = self.desired_playhead
self.last_frame = rospy.Time.from_sec(time.time())
self.last_playhead = self._timeline_frame.playhead
# Recording
def record_bag(self, filename, all=True, topics=[], regex=False, limit=0):
try:
self._recorder = Recorder(filename,
bag_lock=self._bag_lock,
all=all,
topics=topics,
regex=regex,
limit=limit)
except Exception as ex:
qWarning('Error opening bag for recording [%s]: %s' %
(filename, str(ex)))
return
self._recorder.add_listener(self._message_recorded)
self.add_bag(self._recorder.bag)
self._recorder.start()
self.wrap = False
self._timeline_frame._index_cache_thread.period = 0.1
self.update()
def toggle_recording(self):
if self._recorder:
self._recorder.toggle_paused()
self.update()
def _message_recorded(self, topic, msg, t):
if self._timeline_frame._start_stamp is None:
self._timeline_frame._start_stamp = t
self._timeline_frame._end_stamp = t
self._timeline_frame._playhead = t
elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp:
self._timeline_frame._end_stamp = t
if not self._timeline_frame.topics or topic not in self._timeline_frame.topics:
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
)
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
if self._timeline_frame._stamp_left is None:
self.reset_zoom()
# Notify the index caching thread that it has work to do
with self._timeline_frame.index_cache_cv:
self._timeline_frame.invalidated_caches.add(topic)
self._timeline_frame.index_cache_cv.notify()
if topic in self._listeners:
for listener in self._listeners[topic]:
try:
listener.timeline_changed()
except Exception as ex:
qWarning('Error calling timeline_changed on %s: %s' %
(type(listener), str(ex)))
# Views / listeners
def add_view(self, topic, frame):
self._views.append(frame)
def has_listeners(self, topic):
return topic in self._listeners
def add_listener(self, topic, listener):
self._listeners.setdefault(topic, []).append(listener)
self._message_listener_threads[(topic,
listener)] = MessageListenerThread(
self, topic, listener)
# Notify the message listeners
self._message_loaders[topic].reset()
with self._playhead_positions_cvs[topic]:
self._playhead_positions_cvs[topic].notify_all()
self.update()
def remove_listener(self, topic, listener):
topic_listeners = self._listeners.get(topic)
if topic_listeners is not None and listener in topic_listeners:
topic_listeners.remove(listener)
if len(topic_listeners) == 0:
del self._listeners[topic]
# Stop the message listener thread
if (topic, listener) in self._message_listener_threads:
self._message_listener_threads[(topic, listener)].stop()
del self._message_listener_threads[(topic, listener)]
self.update()
### Playhead
# property: play_speed
def _get_play_speed(self):
if self._timeline_frame._paused:
return 0.0
return self._play_speed
def _set_play_speed(self, play_speed):
if play_speed == self._play_speed:
return
if play_speed > 0.0:
self._play_speed = min(self._max_play_speed,
max(self._min_play_speed, play_speed))
elif play_speed < 0.0:
self._play_speed = max(-self._max_play_speed,
min(-self._min_play_speed, play_speed))
else:
self._play_speed = play_speed
if self._play_speed < 1.0:
self.stick_to_end = False
self.update()
play_speed = property(_get_play_speed, _set_play_speed)
def toggle_play(self):
if self._play_speed != 0.0:
self.play_speed = 0.0
else:
self.play_speed = 1.0
def navigate_play(self):
self.play_speed = 1.0
self.last_frame = rospy.Time.from_sec(time.time())
self.last_playhead = self._timeline_frame.playhead
self._play_timer.start()
def navigate_stop(self):
self.play_speed = 0.0
self._play_timer.stop()
def navigate_previous(self):
self.navigate_stop()
self._timeline_frame.playhead = self.get_previous_message_time()
self.last_playhead = self._timeline_frame.playhead
def navigate_next(self):
self.navigate_stop()
self._timeline_frame.playhead = self.get_next_message_time()
self.last_playhead = self._timeline_frame.playhead
def navigate_rewind(self):
if self._play_speed < 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = -1.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
def navigate_fastforward(self):
if self._play_speed > 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = 2.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
def navigate_start(self):
self._timeline_frame.playhead = self._timeline_frame.play_region[0]
def navigate_end(self):
self._timeline_frame.playhead = self._timeline_frame.play_region[1]
class BagTimeline(QGraphicsScene):
"""
BagTimeline contains bag files, all information required to display the bag data visualization on the screen
Also handles events
"""
status_bar_changed_signal = Signal()
selected_region_changed = Signal(rospy.Time, rospy.Time)
make_pop_up = Signal()
def __init__(self, context, publish_clock):
"""
:param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext''
"""
super(BagTimeline, self).__init__()
self._bags = []
self._bag_lock = threading.RLock()
self.background_task = None # Display string
self.background_task_cancel = False
# Playing / Recording
self._playhead_lock = threading.RLock()
self._max_play_speed = 1024.0 # fastest X play speed
self._min_play_speed = 1.0 / 1024.0 # slowest X play speed
self._play_speed = 0.0
self._play_all = False
self._playhead_positions_cvs = {}
self._playhead_positions = {} # topic -> (bag, position)
self._message_loaders = {}
self._messages_cvs = {}
self._messages = {} # topic -> (bag, msg_data)
self._message_listener_threads = {
} # listener -> MessageListenerThread
self._player = False
self._publish_clock = publish_clock
self._recorder = None
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
self.wrap = True # should the playhead wrap when it reaches the end?
self.stick_to_end = False # should the playhead stick to the end?
self._play_timer = QTimer()
self._play_timer.timeout.connect(self.on_idle)
self._play_timer.setInterval(3)
# Plugin popup management
self._context = context
self.popups = {}
self._views = []
self._listeners = {}
# Initialize scene
# the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast.
# otherwise a dark qt theme will default it to black and the frame render pen will be unreadable
self.setBackgroundBrush(Qt.white)
self._timeline_frame = TimelineFrame(self)
self._timeline_frame.setPos(0, 0)
self.addItem(self._timeline_frame)
self.background_progress = 0
self.__closed = False
def get_context(self):
"""
:returns: the ROS_GUI context, 'PluginContext'
"""
return self._context
def handle_close(self):
"""
Cleans up the timeline, bag and any threads
"""
# with open("/var/tmp/test.txt", "a") as myfile:
# myfile.write("dan\n")
if self.__closed:
return
else:
self.__closed = True
self._play_timer.stop()
for topic in self._get_topics():
self.stop_publishing(topic)
self._message_loaders[topic].stop()
if self._player:
self._player.stop()
if self._recorder:
self._recorder.stop()
if self.background_task is not None:
self.background_task_cancel = True
self._timeline_frame.handle_close()
for bag in self._bags:
bag.close()
for frame in self._views:
if frame.parent():
self._context.remove_widget(frame)
# Bag Management and access
def add_bag(self, bag):
"""
creates an indexing thread for each new topic in the bag
fixes the boarders and notifies the indexing thread to index the new items bags
:param bag: ros bag file, ''rosbag.bag''
"""
self._bags.append(bag)
bag_topics = bag_helper.get_topics(bag)
new_topics = set(bag_topics) - set(self._timeline_frame.topics)
for topic in new_topics:
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
self._timeline_frame._start_stamp = self._get_start_stamp()
self._timeline_frame._end_stamp = self._get_end_stamp()
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
)
# If this is the first bag, reset the timeline
if self._timeline_frame._stamp_left is None:
self._timeline_frame.reset_timeline()
# Invalidate entire index cache for all topics in this bag
with self._timeline_frame.index_cache_cv:
for topic in bag_topics:
self._timeline_frame.invalidated_caches.add(topic)
if topic in self._timeline_frame.index_cache:
del self._timeline_frame.index_cache[topic]
self._timeline_frame.index_cache_cv.notify()
def file_size(self):
with self._bag_lock:
return sum(b.size for b in self._bags)
#TODO Rethink API and if these need to be visible
def _get_start_stamp(self):
"""
:return: first stamp in the bags, ''rospy.Time''
"""
with self._bag_lock:
start_stamp = None
for bag in self._bags:
bag_start_stamp = bag_helper.get_start_stamp(bag)
if bag_start_stamp is not None and (
start_stamp is None or bag_start_stamp < start_stamp):
start_stamp = bag_start_stamp
return start_stamp
def _get_end_stamp(self):
"""
:return: last stamp in the bags, ''rospy.Time''
"""
with self._bag_lock:
end_stamp = None
for bag in self._bags:
bag_end_stamp = bag_helper.get_end_stamp(bag)
if bag_end_stamp is not None and (end_stamp is None or
bag_end_stamp > end_stamp):
end_stamp = bag_end_stamp
return end_stamp
def _get_topics(self):
"""
:return: sorted list of topic names, ''list(str)''
"""
with self._bag_lock:
topics = set()
for bag in self._bags:
for topic in bag_helper.get_topics(bag):
topics.add(topic)
return sorted(topics)
def _get_topics_by_datatype(self):
"""
:return: dict of list of topics for each datatype, ''dict(datatype:list(topic))''
"""
with self._bag_lock:
topics_by_datatype = {}
for bag in self._bags:
for datatype, topics in bag_helper.get_topics_by_datatype(
bag).items():
topics_by_datatype.setdefault(datatype, []).extend(topics)
return topics_by_datatype
def get_datatype(self, topic):
"""
:return: datatype associated with a topic, ''str''
:raises: if there are multiple datatypes assigned to a single topic, ''Exception''
"""
with self._bag_lock:
datatype = None
for bag in self._bags:
bag_datatype = bag_helper.get_datatype(bag, topic)
if datatype and bag_datatype and (bag_datatype != datatype):
raise Exception(
'topic %s has multiple datatypes: %s and %s' %
(topic, datatype, bag_datatype))
if bag_datatype:
datatype = bag_datatype
return datatype
def get_entries(self, topics, start_stamp, end_stamp):
"""
generator function for bag entries
:param topics: list of topics to query, ''list(str)''
:param start_stamp: stamp to start at, ''rospy.Time''
:param end_stamp: stamp to end at, ''rospy,Time''
:returns: entries the bag file, ''msg''
"""
with self._bag_lock:
from rosbag import bag # for _mergesort
bag_entries = []
for b in self._bags:
bag_start_time = bag_helper.get_start_stamp(b)
if bag_start_time is not None and bag_start_time > end_stamp:
continue
bag_end_time = bag_helper.get_end_stamp(b)
if bag_end_time is not None and bag_end_time < start_stamp:
continue
connections = list(b._get_connections(topics))
bag_entries.append(
b._get_entries(connections, start_stamp, end_stamp))
for entry, _ in bag._mergesort(bag_entries,
key=lambda entry: entry.time):
yield entry
def get_entries_with_bags(self, topic, start_stamp, end_stamp):
"""
generator function for bag entries
:param topics: list of topics to query, ''list(str)''
:param start_stamp: stamp to start at, ''rospy.Time''
:param end_stamp: stamp to end at, ''rospy,Time''
:returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
from rosbag import bag # for _mergesort
bag_entries = []
bag_by_iter = {}
for b in self._bags:
bag_start_time = bag_helper.get_start_stamp(b)
if bag_start_time is not None and bag_start_time > end_stamp:
continue
bag_end_time = bag_helper.get_end_stamp(b)
if bag_end_time is not None and bag_end_time < start_stamp:
continue
connections = list(b._get_connections(topic))
it = iter(b._get_entries(connections, start_stamp, end_stamp))
bag_by_iter[it] = b
bag_entries.append(it)
for entry, it in bag._mergesort(bag_entries,
key=lambda entry: entry.time):
yield bag_by_iter[it], entry
def get_entry(self, t, topic):
"""
Access a bag entry
:param t: time, ''rospy.Time''
:param topic: the topic to be accessed, ''str''
:return: tuple of (bag, entry) corisponding to time t and topic, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
entry_bag, entry = None, None
for bag in self._bags:
bag_entry = bag._get_entry(t, bag._get_connections(topic))
if bag_entry and (not entry or bag_entry.time > entry.time):
entry_bag, entry = bag, bag_entry
return entry_bag, entry
def get_entry_before(self, t):
"""
Access a bag entry
:param t: time, ''rospy.Time''
:return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
entry_bag, entry = None, None
for bag in self._bags:
bag_entry = bag._get_entry(t - rospy.Duration(0, 1),
bag._get_connections())
if bag_entry and (not entry or bag_entry.time < entry.time):
entry_bag, entry = bag, bag_entry
return entry_bag, entry
def get_entry_after(self, t):
"""
Access a bag entry
:param t: time, ''rospy.Time''
:return: tuple of (bag, entry) corisponding to time t, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
entry_bag, entry = None, None
for bag in self._bags:
bag_entry = bag._get_entry_after(t, bag._get_connections())
if bag_entry and (not entry or bag_entry.time < entry.time):
entry_bag, entry = bag, bag_entry
return entry_bag, entry
def get_next_message_time(self):
"""
:return: time of the next message after the current playhead position,''rospy.Time''
"""
if self._timeline_frame.playhead is None:
return None
_, entry = self.get_entry_after(self._timeline_frame.playhead)
if entry is None:
return self._timeline_frame._start_stamp
return entry.time
# def get_previous_message_time(self):
# """
# :return: time of the next message before the current playhead position,''rospy.Time''
# """
# if self._timeline_frame.playhead is None:
# return None
#
# _, entry = self.get_entry_before(self._timeline_frame.playhead)
# if entry is None:
# return self._timeline_frame._end_stamp
#
# return entry.time
def resume(self):
if (self._player):
self._player.resume()
### Copy messages to...
def start_background_task(self, background_task):
"""
Verify that a background task is not currently running before starting a new one
:param background_task: name of the background task, ''str''
"""
if self.background_task is not None:
QMessageBox(
QMessageBox.Warning, 'Exclamation',
'Background operation already running:\n\n%s' %
self.background_task, QMessageBox.Ok).exec_()
return False
self.background_task = background_task
self.background_task_cancel = False
return True
def stop_background_task(self):
self.background_task = None
def copy_region_to_bag(self, filename):
if len(self._bags) > 0:
self._export_region(filename, self._timeline_frame.topics,
self._timeline_frame.play_region[0],
self._timeline_frame.play_region[1])
def _export_region(self, path, topics, start_stamp, end_stamp):
"""
Starts a thread to save the current selection to a new bag file
:param path: filesystem path to write to, ''str''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
if not self.start_background_task('Copying messages to "%s"' % path):
return
# TODO implement a status bar area with information on the current save status
bag_entries = list(
self.get_entries_with_bags(topics, start_stamp, end_stamp))
if self.background_task_cancel:
return
# Get the total number of messages to copy
total_messages = len(bag_entries)
# If no messages, prompt the user and return
if total_messages == 0:
QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found',
QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Open the path for writing
try:
export_bag = rosbag.Bag(path, 'w')
except Exception:
QMessageBox(QMessageBox.Warning, 'rqt_bag',
'Error opening bag file [%s] for writing' % path,
QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Run copying in a background thread
self._export_thread = threading.Thread(target=self._run_export_region,
args=(export_bag, topics,
start_stamp, end_stamp,
bag_entries))
self._export_thread.start()
def _run_export_region(self, export_bag, topics, start_stamp, end_stamp,
bag_entries):
"""
Threaded function that saves the current selection to a new bag file
:param export_bag: bagfile to write to, ''rosbag.bag''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
total_messages = len(bag_entries)
update_step = max(1, total_messages / 100)
message_num = 1
progress = 0
# Write out the messages
for bag, entry in bag_entries:
if self.background_task_cancel:
break
try:
topic, msg, t = self.read_message(bag, entry.position)
export_bag.write(topic, msg, t)
except Exception as ex:
qWarning('Error exporting message at position %s: %s' %
(str(entry.position), str(ex)))
export_bag.close()
self.stop_background_task()
return
if message_num % update_step == 0 or message_num == total_messages:
new_progress = int(100.0 *
(float(message_num) / total_messages))
if new_progress != progress:
progress = new_progress
if not self.background_task_cancel:
self.background_progress = progress
self.status_bar_changed_signal.emit()
message_num += 1
# Close the bag
try:
self.background_progress = 0
self.status_bar_changed_signal.emit()
export_bag.close()
except Exception as ex:
QMessageBox(
QMessageBox.Warning, 'rqt_bag',
'Error closing bag file [%s]: %s' %
(export_bag.filename, str(ex)), QMessageBox.Ok).exec_()
self.stop_background_task()
def read_message(self, bag, position):
with self._bag_lock:
return bag._read_message(position)
### Mouse events
def on_mouse_down(self, event):
if event.buttons() == Qt.LeftButton:
self._timeline_frame.on_left_down(event)
elif event.buttons() == Qt.MidButton:
self._timeline_frame.on_middle_down(event)
elif event.buttons() == Qt.RightButton:
topic = self._timeline_frame.map_y_to_topic(
self.views()[0].mapToScene(event.pos()).y())
TimelinePopupMenu(self, event, topic)
def on_mouse_up(self, event):
self._timeline_frame.on_mouse_up(event)
def on_mouse_move(self, event):
self._timeline_frame.on_mouse_move(event)
def on_mousewheel(self, event):
self._timeline_frame.on_mousewheel(event)
# Zooming
def zoom_in(self):
self._timeline_frame.zoom_in()
def zoom_out(self):
self._timeline_frame.zoom_out()
def reset_zoom(self):
self._timeline_frame.reset_zoom()
def translate_timeline_left(self):
self._timeline_frame.translate_timeline_left()
def translate_timeline_right(self):
self._timeline_frame.translate_timeline_right()
### Publishing
def is_publishing(self, topic):
return self._player and self._player.is_publishing(topic)
def start_publishing(self, topic):
if not self._player and not self._create_player():
return False
self._player.start_publishing(topic)
return True
def stop_publishing(self, topic):
if not self._player:
return False
self._player.stop_publishing(topic)
return True
def _create_player(self):
if not self._player:
try:
self._player = Player(self)
if self._publish_clock:
self._player.start_clock_publishing()
except Exception as ex:
qWarning('Error starting player; aborting publish: %s' %
str(ex))
return False
return True
def set_publishing_state(self, start_publishing):
if start_publishing:
for topic in self._timeline_frame.topics:
if not self.start_publishing(topic):
break
else:
for topic in self._timeline_frame.topics:
self.stop_publishing(topic)
# property: play_all
def _get_play_all(self):
return self._play_all
def _set_play_all(self, play_all):
if play_all == self._play_all:
return
self._play_all = not self._play_all
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
play_all = property(_get_play_all, _set_play_all)
def toggle_play_all(self):
self.play_all = not self.play_all
### Playing
def on_idle(self):
self._step_playhead()
def _step_playhead(self):
"""
moves the playhead to the next position based on the desired position
"""
# Reset when the playing mode switchs
if self._timeline_frame.playhead != self.last_playhead:
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
if self._play_all:
self.step_next_message()
else:
self.step_fixed()
def step_fixed(self):
"""
Moves the playhead a fixed distance into the future based on the current play speed
"""
if self.play_speed == 0.0 or not self._timeline_frame.playhead:
self.last_frame = None
self.last_playhead = None
return
now = rospy.Time.from_sec(time.time())
if self.last_frame:
# Get new playhead
if self.stick_to_end:
new_playhead = self.end_stamp
else:
new_playhead = self._timeline_frame.playhead + rospy.Duration.from_sec(
(now - self.last_frame).to_sec() * self.play_speed)
start_stamp, end_stamp = self._timeline_frame.play_region
if new_playhead > end_stamp:
if self.wrap:
if self.play_speed > 0.0:
new_playhead = start_stamp
else:
new_playhead = end_stamp
else:
new_playhead = end_stamp
if self.play_speed > 0.0:
self.stick_to_end = True
elif new_playhead < start_stamp:
if self.wrap:
if self.play_speed < 0.0:
new_playhead = end_stamp
else:
new_playhead = start_stamp
else:
new_playhead = start_stamp
# Update the playhead
self._timeline_frame.playhead = new_playhead
self.last_frame = now
self.last_playhead = self._timeline_frame.playhead
def step_next_message(self):
"""
Move the playhead to the next message
"""
if self.play_speed <= 0.0 or not self._timeline_frame.playhead:
self.last_frame = None
self.last_playhead = None
return
if self.last_frame:
if not self.desired_playhead:
self.desired_playhead = self._timeline_frame.playhead
else:
delta = rospy.Time.from_sec(time.time()) - self.last_frame
if delta > rospy.Duration.from_sec(0.1):
delta = rospy.Duration.from_sec(0.1)
self.desired_playhead += delta
# Get the occurrence of the next message
next_message_time = self.get_next_message_time()
if next_message_time < self.desired_playhead:
self._timeline_frame.playhead = next_message_time
else:
self._timeline_frame.playhead = self.desired_playhead
self.last_frame = rospy.Time.from_sec(time.time())
self.last_playhead = self._timeline_frame.playhead
### Recording
def record_bag(self, filename, all=True, topics=[], regex=False, limit=0):
try:
self._recorder = Recorder(filename,
bag_lock=self._bag_lock,
all=all,
topics=topics,
regex=regex,
limit=limit)
except Exception as ex:
qWarning('Error opening bag for recording [%s]: %s' %
(filename, str(ex)))
return
self._recorder.add_listener(self._message_recorded)
self.add_bag(self._recorder.bag)
self._recorder.start()
self.wrap = False
self._timeline_frame._index_cache_thread.period = 0.1
self.update()
def restart_recording(self, reindex_bag):
self._BagWidget.apply_restart(reindex_bag)
import os
# self._BagWidget.record_button.setIcon(QIcon.fromTheme('view-refresh'))
# self._BagWidget.record_button.setToolTip("Refresh Screen")
# self._BagWidget.restart_button.setEnabled(False)
#
# self._BagWidget.load_button.setEnabled(True)
# self._BagWidget.history_button.setEnabled(True)
#
# self._BagWidget._restarting = True
# self.handle_close()
# if path != "":
# os.remove(path)
# if restart_flag:
# else:
# self.make_pop_up.emit()
# def apply_record_icon(self):
# self._BagWidget.record_button.setIcon(QIcon.fromTheme('media-record'))
def setBagWidget(self, BagWidget):
self._BagWidget = BagWidget
def toggle_recording(self):
if self._recorder:
self._recorder.toggle_paused(self._BagWidget)
self.update()
def _message_recorded(self, topic, msg, t):
if self._timeline_frame._start_stamp is None:
self._timeline_frame._start_stamp = t
self._timeline_frame._end_stamp = t
self._timeline_frame._playhead = t
elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp:
self._timeline_frame._end_stamp = t
if not self._timeline_frame.topics or topic not in self._timeline_frame.topics:
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype(
)
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
if self._timeline_frame._stamp_left is None:
self.reset_zoom()
# Notify the index caching thread that it has work to do
with self._timeline_frame.index_cache_cv:
self._timeline_frame.invalidated_caches.add(topic)
self._timeline_frame.index_cache_cv.notify()
if topic in self._listeners:
for listener in self._listeners[topic]:
try:
listener.timeline_changed()
except Exception as ex:
qWarning('Error calling timeline_changed on %s: %s' %
(type(listener), str(ex)))
### Views / listeners
def add_view(self, topic, frame):
self._views.append(frame)
def has_listeners(self, topic):
return topic in self._listeners
def add_listener(self, topic, listener):
self._listeners.setdefault(topic, []).append(listener)
self._message_listener_threads[(topic,
listener)] = MessageListenerThread(
self, topic, listener)
# Notify the message listeners
self._message_loaders[topic].reset()
with self._playhead_positions_cvs[topic]:
self._playhead_positions_cvs[topic].notify_all()
self.update()
def remove_listener(self, topic, listener):
topic_listeners = self._listeners.get(topic)
if topic_listeners is not None and listener in topic_listeners:
topic_listeners.remove(listener)
if len(topic_listeners) == 0:
del self._listeners[topic]
# Stop the message listener thread
if (topic, listener) in self._message_listener_threads:
self._message_listener_threads[(topic, listener)].stop()
del self._message_listener_threads[(topic, listener)]
self.update()
### Playhead
# property: play_speed
def _get_play_speed(self):
if self._timeline_frame._paused:
return 0.0
return self._play_speed
def _set_play_speed(self, play_speed):
if play_speed == self._play_speed:
return
if play_speed > 0.0:
self._play_speed = min(self._max_play_speed,
max(self._min_play_speed, play_speed))
elif play_speed < 0.0:
self._play_speed = max(-self._max_play_speed,
min(-self._min_play_speed, play_speed))
else:
self._play_speed = play_speed
if self._play_speed < 1.0:
self.stick_to_end = False
self.update()
play_speed = property(_get_play_speed, _set_play_speed)
def toggle_play(self):
if self._play_speed != 0.0:
self.play_speed = 0.0
else:
self.play_speed = 1.0
# def navigate_play(self):
# self.play_speed = 1.0
# self.last_frame = rospy.Time.from_sec(time.time())
# self.last_playhead = self._timeline_frame.playhead
# self._play_timer.start()
# def navigate_stop(self):
# self.play_speed = 0.0
# self._play_timer.stop()
# def navigate_previous(self):
# self.navigate_stop()
# self._timeline_frame.playhead = self.get_previous_message_time()
# self.last_playhead = self._timeline_frame.playhead
# def navigate_next(self):
# self.navigate_stop()
# self._timeline_frame.playhead = self.get_next_message_time()
# self.last_playhead = self._timeline_frame.playhead
# def navigate_rewind(self):
# if self._play_speed < 0.0:
# new_play_speed = self._play_speed * 2.0
# elif self._play_speed == 0.0:
# new_play_speed = -1.0
# else:
# new_play_speed = self._play_speed * 0.5
#
# self.play_speed = new_play_speed
def navigate_fastforward(self):
if self._play_speed > 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = 2.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
class BarrettDashboard(Plugin):
def __init__(self, context):
super(BarrettDashboard, self).__init__(context)
self._joint_sub = None
# Give QObjects reasonable names
self.setObjectName('BarrettDashboard')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'barrett_dashboard.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('BarrettDashboardPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.context = context
jp_widgets = [getattr(self._widget,'jp_%d' % i) for i in range(7)]
jn_widgets = [getattr(self._widget,'jn_%d' % i) for i in range(7)]
self.joint_widgets = zip(jp_widgets,jn_widgets)
tp_widgets = [getattr(self._widget,'tp_%d' % i) for i in range(7)]
tn_widgets = [getattr(self._widget,'tn_%d' % i) for i in range(7)]
self.torque_widgets = zip(tp_widgets,tn_widgets)
self.joint_signals = []
self.torque_signals = []
for (tp,tn) in self.torque_widgets:
tp.setRange(0.0,1.0,False)
tn.setRange(1.0,0.0,False)
tp.setValue(0.0)
tn.setValue(0.0)
# set random values for testing
#v = (2.0*random.random()) - 1.0
#tp.setValue(v if v >=0 else 0)
#tn.setValue(-v if v <0 else 0)
for (jp,jn) in self.joint_widgets:
jp.setRange(0.0,1.0,False)
jn.setRange(1.0,0.0,False)
jp.setValue(0.0)
jn.setValue(0.0)
# set random values for testing
#v = (2.0*random.random()) - 1.0
#jp.setValue(v if v >=0 else 0)
#jn.setValue(-v if v <0 else 0)
self.barrett_green = QColor(87,186,142)
self.barrett_green_dark = self.barrett_green.darker()
self.barrett_green_light = self.barrett_green.lighter()
self.barrett_blue = QColor(80,148,204)
self.barrett_blue_dark = self.barrett_blue.darker()
self.barrett_blue_light = self.barrett_blue.lighter()
self.barrett_red = QColor(232,47,47)
self.barrett_red_dark = self.barrett_red.darker()
self.barrett_red_light = self.barrett_red.lighter()
self.barrett_orange = QColor(255,103,43)
self.barrett_orange_dark = self.barrett_orange.darker()
#background_color = p.mid().color()
joint_bg_color = self.barrett_blue_dark.darker()
joint_fill_color = self.barrett_blue
joint_alarm_color = self.barrett_blue #self.barrett_blue_light
torque_bg_color = self.barrett_green_dark.darker()
torque_fill_color = self.barrett_green
torque_alarm_color = self.barrett_orange #self.barrett_green_light
temp_bg_color = self.barrett_red_dark.darker()
temp_fill_color = self.barrett_orange
temp_alarm_color = self.barrett_red
for w in jp_widgets + jn_widgets:
w.setAlarmLevel(0.80)
w.setFillColor(joint_fill_color)
w.setAlarmColor(joint_alarm_color)
p = w.palette()
p.setColor(tp.backgroundRole(), joint_bg_color)
w.setPalette(p)
for w in tp_widgets + tn_widgets:
w.setAlarmLevel(0.66)
w.setFillColor(torque_fill_color)
w.setAlarmColor(torque_alarm_color)
p = w.palette()
p.setColor(tp.backgroundRole(), torque_bg_color)
w.setPalette(p)
self._widget.hand_temp.setAlarmLevel(0.66)
self._widget.hand_temp.setFillColor(temp_fill_color)
self._widget.hand_temp.setAlarmColor(temp_alarm_color)
p = self._widget.hand_temp.palette()
p.setColor(self._widget.hand_temp.backgroundRole(), temp_bg_color)
self._widget.hand_temp.setPalette(p)
self._widget.hand_temp.setOrientation(Qt.Horizontal, QwtThermo.RightScale)
self._widget.jf_0.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % (
joint_bg_color.red(), joint_bg_color.green(), joint_bg_color.blue(), joint_fill_color.red(), joint_fill_color.green(), joint_fill_color.blue()))
self.urdf = rospy.get_param('robot_description')
self.robot = URDF()
self.robot = self.robot.from_xml_string(self.urdf)
self.pos_norm = [0] * 7
self.torque_norm = [0] * 7
self._joint_sub = rospy.Subscriber(
'joint_states',
sensor_msgs.msg.JointState,
self._joint_state_cb)
self._status_sub = rospy.Subscriber(
'status',
oro_barrett_msgs.msg.BarrettStatus,
self._status_cb)
self._hand_status_sub = rospy.Subscriber(
'hand/status',
oro_barrett_msgs.msg.BHandStatus,
self._hand_status_cb)
self._update_status(-1)
self.safety_mode = -1
self.run_mode = 0
self.hand_run_mode = 0
self.hand_min_temperature = 40.0
self.hand_max_temperature = 65.0
self.hand_temperature = 0.0
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widget_values)
self.update_timer.start()
self.set_home_client = actionlib.SimpleActionClient(
'wam/set_home_action',
oro_barrett_msgs.msg.SetHomeAction)
self.set_mode_client = actionlib.SimpleActionClient(
'wam/set_mode_action',
oro_barrett_msgs.msg.SetModeAction)
self._widget.button_set_home.clicked[bool].connect(self._handle_set_home_clicked)
self._widget.button_idle_wam.clicked[bool].connect(self._handle_idle_wam_clicked)
self._widget.button_run_wam.clicked[bool].connect(self._handle_run_wam_clicked)
self.bhand_init_client = actionlib.SimpleActionClient(
'hand/initialize_action',
oro_barrett_msgs.msg.BHandInitAction)
self.bhand_set_mode_client = actionlib.SimpleActionClient(
'hand/set_mode_action',
oro_barrett_msgs.msg.BHandSetModeAction)
self.grasp_client = actionlib.SimpleActionClient(
'grasp', BHandGraspAction)
self.release_client = actionlib.SimpleActionClient(
'release', BHandReleaseAction)
self.spread_client = actionlib.SimpleActionClient(
'spread', BHandSpreadAction)
self._widget.button_initialize_hand.clicked[bool].connect(self._handle_initialize_hand_clicked)
self._widget.button_idle_hand.clicked[bool].connect(self._handle_idle_hand_clicked)
self._widget.button_run_hand.clicked[bool].connect(self._handle_run_hand_clicked)
self._widget.button_release_hand.clicked[bool].connect(self._handle_release_hand_clicked)
self._widget.button_grasp_hand.clicked[bool].connect(self._handle_grasp_hand_clicked)
self._widget.button_set_spread.clicked[bool].connect(self._handle_spread_hand_clicked)
self._widget.resizeEvent = self._handle_resize
def _handle_resize(self, event):
for i,((w1,w2),(w3,w4)) in enumerate(zip(self.joint_widgets, self.torque_widgets)):
width = 0.8*getattr(self._widget, 'jcc_%d'%i).contentsRect().width()
w1.setPipeWidth(width)
w2.setPipeWidth(width)
w3.setPipeWidth(width)
w4.setPipeWidth(width)
def _handle_set_home_clicked(self, checked):
if checked:
self.set_home()
def _update_status(self, status):
if status == -1:
self._widget.safety_mode.setText('UNKNOWN SAFETY MODE')
self._widget.safety_mode.setToolTip('The WAM is in an unknown state. Proceed with caution.')
color = QColor(200,200,200)
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
elif status == 0:
self._widget.safety_mode.setText('E-STOP')
self._widget.safety_mode.setToolTip('The WAM is stopped and unpowered. Determine the source of the fault and reset the arm to continue using it.')
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_red
else:
if not self.homed:
self._widget.safety_mode.setText('UNCALIBRATED')
self._widget.safety_mode.setToolTip('The WAM is not calibrated. Please place it in the calibration posture and click the "Calibrate" button.')
self._widget.button_set_home.setEnabled(True)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_orange
else:
if status == 1:
self._widget.safety_mode.setText('IDLE')
self._widget.safety_mode.setToolTip('The WAM is running but all joints are passive. It is safe to home the arm.')
self._widget.button_set_home.setEnabled(True)
self._widget.button_idle_wam.setEnabled(True)
self._widget.button_run_wam.setEnabled(True)
color = self.barrett_blue
elif status == 2:
self._widget.safety_mode.setText('ACTIVE')
self._widget.safety_mode.setToolTip('The WAM is running and all joints are active. Proceed with caution.')
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_green
darker = color.darker()
lighter = color.lighter()
self._widget.safety_mode.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % (
color.red(), color.green(), color.blue(), lighter.red(), lighter.green(), lighter.blue()))
def _update_widget_values(self):
if self.safety_mode in (oro_barrett_msgs.msg.SafetyMode.ACTIVE, oro_barrett_msgs.msg.SafetyMode.IDLE):
for (v,(tp,tn)) in zip(self.torque_norm,self.torque_widgets):
tp.setEnabled(True)
tn.setEnabled(True)
tp.setValue(v if v >=0 else 0)
tn.setValue(-v if v <0 else 0)
for (v,(jp,jn)) in zip(self.pos_norm,self.joint_widgets):
jp.setEnabled(True)
jn.setEnabled(True)
jp.setValue(v if v >=0 else 0)
jn.setValue(-v if v <0 else 0)
else:
for (p,n) in self.joint_widgets + self.torque_widgets:
p.setEnabled(True)
n.setEnabled(True)
self._widget.hand_temp.setValue((self.hand_temperature-self.hand_min_temperature)/(self.hand_max_temperature-self.hand_min_temperature))
self._update_status(self.safety_mode)
self._update_buttons(self.run_mode, self.hand_run_mode)
def _update_buttons(self, run_mode, hand_run_mode):
if run_mode == oro_barrett_msgs.msg.RunMode.IDLE:
self._widget.button_idle_wam.setChecked(True)
self._widget.button_run_wam.setChecked(False)
else:
self._widget.button_idle_wam.setChecked(False)
self._widget.button_run_wam.setChecked(True)
if hand_run_mode == oro_barrett_msgs.msg.RunMode.RUN:
self._widget.button_idle_hand.setChecked(False)
self._widget.button_run_hand.setChecked(True)
else:
self._widget.button_idle_hand.setChecked(True)
self._widget.button_run_hand.setChecked(False)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
#def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def _joint_state_cb(self, msg):
joint_pos_map = dict(zip(msg.name, msg.position))
for (name,pos,torque,i) in zip(msg.name,msg.position,msg.effort,range(7)):
joint = self.robot.joint_map[name]
self.pos_norm[i] = 2.0*((pos-joint.limit.lower)/(joint.limit.upper-joint.limit.lower)) - 1.0
self.torque_norm[i] = torque/joint.limit.effort
def _status_cb(self, msg):
self.safety_mode = msg.safety_mode.value
self.run_mode = msg.run_mode.value
self.homed = msg.homed
if self.safety_mode == oro_barrett_msgs.msg.SafetyMode.ACTIVE:
self._widget.button_initialize_hand.setEnabled(False)
elif self.safety_mode == oro_barrett_msgs.msg.SafetyMode.ESTOP:
self._widget.button_initialize_hand.setEnabled(False)
self._widget.button_idle_hand.setEnabled(False)
self._widget.button_run_hand.setEnabled(False)
def _hand_status_cb(self, msg):
self.hand_initialized = msg.initialized
self.hand_temperature = max(msg.temperature)
self.hand_run_mode = msg.run_mode.value
def _handle_set_home_clicked(self, checked):
goal = oro_barrett_msgs.msg.SetHomeGoal()
self.set_home_client.send_goal(goal)
def _handle_idle_wam_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.SetModeGoal()
goal.mode.value = oro_barrett_msgs.msg.RunMode.IDLE
self.set_mode_client.send_goal(goal)
def _handle_run_wam_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.SetModeGoal()
goal.mode.value = oro_barrett_msgs.msg.RunMode.RUN
self.set_mode_client.send_goal(goal)
def _handle_initialize_hand_clicked(self, checked):
if self.safety_mode == oro_barrett_msgs.msg.SafetyMode.IDLE:
goal = oro_barrett_msgs.msg.BHandInitGoal()
self.bhand_init_client.send_goal(goal)
def _handle_run_hand_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.BHandSetModeGoal()
goal.run_mode.value = oro_barrett_msgs.msg.RunMode.RUN
self.bhand_set_mode_client.send_goal(goal)
def _handle_idle_hand_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.BHandSetModeGoal()
goal.run_mode.value = oro_barrett_msgs.msg.RunMode.IDLE
self.bhand_set_mode_client.send_goal(goal)
def _get_grasp_mask(self):
return [ self._widget.button_use_f1.isChecked(),
self._widget.button_use_f2.isChecked(),
self._widget.button_use_f3.isChecked()]
def _handle_release_hand_clicked(self, checked):
goal = BHandReleaseGoal()
goal.release_mask = self._get_grasp_mask()
goal.release_speed = [3.0, 3.0, 3.0]
self.release_client.send_goal(goal)
def _handle_grasp_hand_clicked(self, checked):
goal = BHandGraspGoal()
goal.grasp_mask = self._get_grasp_mask()
goal.grasp_speed = [1.0, 1.0, 1.0]
goal.grasp_effort = [1.0, 1.0, 1.0]
goal.min_fingertip_radius = 0.0
self.grasp_client.send_goal(goal)
def _handle_spread_hand_clicked(self, checked):
goal = BHandSpreadGoal()
goal.spread_position = self._widget.spread_slider.value()/1000.0*3.141
self.spread_client.send_goal(goal)
class BagTimeline(QGraphicsScene):
"""
BagTimeline contains bag files, all information required to display the bag data visualization on the screen
Also handles events
"""
status_bar_changed_signal = Signal()
selected_region_changed = Signal(rospy.Time, rospy.Time)
def __init__(self, context, publish_clock):
"""
:param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext''
"""
super(BagTimeline, self).__init__()
self._bags = []
self._bag_lock = threading.RLock()
self.background_task = None # Display string
self.background_task_cancel = False
# Playing / Recording
self._playhead_lock = threading.RLock()
self._max_play_speed = 1024.0 # fastest X play speed
self._min_play_speed = 1.0 / 1024.0 # slowest X play speed
self._play_speed = 0.0
self._play_all = False
self._playhead_positions_cvs = {}
self._playhead_positions = {} # topic -> (bag, position)
self._message_loaders = {}
self._messages_cvs = {}
self._messages = {} # topic -> (bag, msg_data)
self._message_listener_threads = {} # listener -> MessageListenerThread
self._player = False
self._publish_clock = publish_clock
self._recorder = None
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
self.wrap = True # should the playhead wrap when it reaches the end?
self.stick_to_end = False # should the playhead stick to the end?
self._play_timer = QTimer()
self._play_timer.timeout.connect(self.on_idle)
self._play_timer.setInterval(3)
# Plugin popup management
self._context = context
self.popups = {}
self._views = []
self._listeners = {}
# Initialize scene
# the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast.
# otherwise a dark qt theme will default it to black and the frame render pen will be unreadable
self.setBackgroundBrush(Qt.white)
self._timeline_frame = TimelineFrame(self)
self._timeline_frame.setPos(0, 0)
self.addItem(self._timeline_frame)
self.background_progress = 0
self.__closed = False
def get_context(self):
"""
:returns: the ROS_GUI context, 'PluginContext'
"""
return self._context
def handle_close(self):
"""
Cleans up the timeline, bag and any threads
"""
if self.__closed:
return
else:
self.__closed = True
self._play_timer.stop()
for topic in self._get_topics():
self.stop_publishing(topic)
self._message_loaders[topic].stop()
if self._player:
self._player.stop()
if self._recorder:
self._recorder.stop()
if self.background_task is not None:
self.background_task_cancel = True
self._timeline_frame.handle_close()
for bag in self._bags:
bag.close()
for frame in self._views:
if frame.parent():
self._context.remove_widget(frame)
# Bag Management and access
def add_bag(self, bag):
"""
creates an indexing thread for each new topic in the bag
fixes the boarders and notifies the indexing thread to index the new items bags
:param bag: ros bag file, ''rosbag.bag''
"""
self._bags.append(bag)
bag_topics = bag_helper.get_topics(bag)
new_topics = set(bag_topics) - set(self._timeline_frame.topics)
for topic in new_topics:
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
self._timeline_frame._start_stamp = self._get_start_stamp()
self._timeline_frame._end_stamp = self._get_end_stamp()
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype()
# If this is the first bag, reset the timeline
if self._timeline_frame._stamp_left is None:
self._timeline_frame.reset_timeline()
# Invalidate entire index cache for all topics in this bag
with self._timeline_frame.index_cache_cv:
for topic in bag_topics:
self._timeline_frame.invalidated_caches.add(topic)
if topic in self._timeline_frame.index_cache:
del self._timeline_frame.index_cache[topic]
self._timeline_frame.index_cache_cv.notify()
def file_size(self):
with self._bag_lock:
return sum(b.size for b in self._bags)
#TODO Rethink API and if these need to be visible
def _get_start_stamp(self):
"""
:return: first stamp in the bags, ''rospy.Time''
"""
with self._bag_lock:
start_stamp = None
for bag in self._bags:
bag_start_stamp = bag_helper.get_start_stamp(bag)
if bag_start_stamp is not None and (start_stamp is None or bag_start_stamp < start_stamp):
start_stamp = bag_start_stamp
return start_stamp
def _get_end_stamp(self):
"""
:return: last stamp in the bags, ''rospy.Time''
"""
with self._bag_lock:
end_stamp = None
for bag in self._bags:
bag_end_stamp = bag_helper.get_end_stamp(bag)
if bag_end_stamp is not None and (end_stamp is None or bag_end_stamp > end_stamp):
end_stamp = bag_end_stamp
return end_stamp
def _get_topics(self):
"""
:return: sorted list of topic names, ''list(str)''
"""
with self._bag_lock:
topics = set()
for bag in self._bags:
for topic in bag_helper.get_topics(bag):
topics.add(topic)
return sorted(topics)
def _get_topics_by_datatype(self):
"""
:return: dict of list of topics for each datatype, ''dict(datatype:list(topic))''
"""
with self._bag_lock:
topics_by_datatype = {}
for bag in self._bags:
for datatype, topics in bag_helper.get_topics_by_datatype(bag).items():
topics_by_datatype.setdefault(datatype, []).extend(topics)
return topics_by_datatype
def get_datatype(self, topic):
"""
:return: datatype associated with a topic, ''str''
:raises: if there are multiple datatypes assigned to a single topic, ''Exception''
"""
with self._bag_lock:
datatype = None
for bag in self._bags:
bag_datatype = bag_helper.get_datatype(bag, topic)
if datatype and bag_datatype and (bag_datatype != datatype):
raise Exception('topic %s has multiple datatypes: %s and %s' % (topic, datatype, bag_datatype))
if bag_datatype:
datatype = bag_datatype
return datatype
def get_entries(self, topics, start_stamp, end_stamp):
"""
generator function for bag entries
:param topics: list of topics to query, ''list(str)''
:param start_stamp: stamp to start at, ''rospy.Time''
:param end_stamp: stamp to end at, ''rospy,Time''
:returns: entries the bag file, ''msg''
"""
with self._bag_lock:
from rosbag import bag # for _mergesort
bag_entries = []
for b in self._bags:
bag_start_time = bag_helper.get_start_stamp(b)
if bag_start_time is not None and bag_start_time > end_stamp:
continue
bag_end_time = bag_helper.get_end_stamp(b)
if bag_end_time is not None and bag_end_time < start_stamp:
continue
connections = list(b._get_connections(topics))
bag_entries.append(b._get_entries(connections, start_stamp, end_stamp))
for entry, _ in bag._mergesort(bag_entries, key=lambda entry: entry.time):
yield entry
def get_entries_with_bags(self, topic, start_stamp, end_stamp):
"""
generator function for bag entries
:param topics: list of topics to query, ''list(str)''
:param start_stamp: stamp to start at, ''rospy.Time''
:param end_stamp: stamp to end at, ''rospy,Time''
:returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
from rosbag import bag # for _mergesort
bag_entries = []
bag_by_iter = {}
for b in self._bags:
bag_start_time = bag_helper.get_start_stamp(b)
if bag_start_time is not None and bag_start_time > end_stamp:
continue
bag_end_time = bag_helper.get_end_stamp(b)
if bag_end_time is not None and bag_end_time < start_stamp:
continue
connections = list(b._get_connections(topic))
it = iter(b._get_entries(connections, start_stamp, end_stamp))
bag_by_iter[it] = b
bag_entries.append(it)
for entry, it in bag._mergesort(bag_entries, key=lambda entry: entry.time):
yield bag_by_iter[it], entry
def get_entry(self, t, topic):
"""
Access a bag entry
:param t: time, ''rospy.Time''
:param topic: the topic to be accessed, ''str''
:return: tuple of (bag, entry) corisponding to time t and topic, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
entry_bag, entry = None, None
for bag in self._bags:
bag_entry = bag._get_entry(t, bag._get_connections(topic))
if bag_entry and (not entry or bag_entry.time > entry.time):
entry_bag, entry = bag, bag_entry
return entry_bag, entry
def get_entry_before(self, t):
"""
Access a bag entry
:param t: time, ''rospy.Time''
:return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
entry_bag, entry = None, None
for bag in self._bags:
bag_entry = bag._get_entry(t-rospy.Duration(0,1), bag._get_connections())
if bag_entry and (not entry or bag_entry.time < entry.time):
entry_bag, entry = bag, bag_entry
return entry_bag, entry
def get_entry_after(self, t):
"""
Access a bag entry
:param t: time, ''rospy.Time''
:return: tuple of (bag, entry) corisponding to time t, ''(rosbag.bag, msg)''
"""
with self._bag_lock:
entry_bag, entry = None, None
for bag in self._bags:
bag_entry = bag._get_entry_after(t, bag._get_connections())
if bag_entry and (not entry or bag_entry.time < entry.time):
entry_bag, entry = bag, bag_entry
return entry_bag, entry
def get_next_message_time(self):
"""
:return: time of the next message after the current playhead position,''rospy.Time''
"""
if self._timeline_frame.playhead is None:
return None
_, entry = self.get_entry_after(self._timeline_frame.playhead)
if entry is None:
return self._timeline_frame._start_stamp
return entry.time
def get_previous_message_time(self):
"""
:return: time of the next message before the current playhead position,''rospy.Time''
"""
if self._timeline_frame.playhead is None:
return None
_, entry = self.get_entry_before(self._timeline_frame.playhead)
if entry is None:
return self._timeline_frame._end_stamp
return entry.time
def resume(self):
if (self._player):
self._player.resume()
### Copy messages to...
def start_background_task(self, background_task):
"""
Verify that a background task is not currently running before starting a new one
:param background_task: name of the background task, ''str''
"""
if self.background_task is not None:
QMessageBox(QMessageBox.Warning, 'Exclamation', 'Background operation already running:\n\n%s' % self.background_task, QMessageBox.Ok).exec_()
return False
self.background_task = background_task
self.background_task_cancel = False
return True
def stop_background_task(self):
self.background_task = None
def copy_region_to_bag(self, filename):
if len(self._bags) > 0:
self._export_region(filename, self._timeline_frame.topics, self._timeline_frame.play_region[0], self._timeline_frame.play_region[1])
def _export_region(self, path, topics, start_stamp, end_stamp):
"""
Starts a thread to save the current selection to a new bag file
:param path: filesystem path to write to, ''str''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
if not self.start_background_task('Copying messages to "%s"' % path):
return
# TODO implement a status bar area with information on the current save status
bag_entries = list(self.get_entries_with_bags(topics, start_stamp, end_stamp))
if self.background_task_cancel:
return
# Get the total number of messages to copy
total_messages = len(bag_entries)
# If no messages, prompt the user and return
if total_messages == 0:
QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found', QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Open the path for writing
try:
export_bag = rosbag.Bag(path, 'w')
except Exception:
QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error opening bag file [%s] for writing' % path, QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Run copying in a background thread
self._export_thread = threading.Thread(target=self._run_export_region, args=(export_bag, topics, start_stamp, end_stamp, bag_entries))
self._export_thread.start()
def _run_export_region(self, export_bag, topics, start_stamp, end_stamp, bag_entries):
"""
Threaded function that saves the current selection to a new bag file
:param export_bag: bagfile to write to, ''rosbag.bag''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
total_messages = len(bag_entries)
update_step = max(1, total_messages / 100)
message_num = 1
progress = 0
# Write out the messages
for bag, entry in bag_entries:
if self.background_task_cancel:
break
try:
topic, msg, t = self.read_message(bag, entry.position)
export_bag.write(topic, msg, t)
except Exception as ex:
qWarning('Error exporting message at position %s: %s' % (str(entry.position), str(ex)))
export_bag.close()
self.stop_background_task()
return
if message_num % update_step == 0 or message_num == total_messages:
new_progress = int(100.0 * (float(message_num) / total_messages))
if new_progress != progress:
progress = new_progress
if not self.background_task_cancel:
self.background_progress = progress
self.status_bar_changed_signal.emit()
message_num += 1
# Close the bag
try:
self.background_progress = 0
self.status_bar_changed_signal.emit()
export_bag.close()
except Exception as ex:
QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error closing bag file [%s]: %s' % (export_bag.filename, str(ex)), QMessageBox.Ok).exec_()
self.stop_background_task()
def read_message(self, bag, position):
with self._bag_lock:
return bag._read_message(position)
### Mouse events
def on_mouse_down(self, event):
if event.buttons() == Qt.LeftButton:
self._timeline_frame.on_left_down(event)
elif event.buttons() == Qt.MidButton:
self._timeline_frame.on_middle_down(event)
elif event.buttons() == Qt.RightButton:
topic = self._timeline_frame.map_y_to_topic(self.views()[0].mapToScene(event.pos()).y())
TimelinePopupMenu(self, event, topic)
def on_mouse_up(self, event):
self._timeline_frame.on_mouse_up(event)
def on_mouse_move(self, event):
self._timeline_frame.on_mouse_move(event)
def on_mousewheel(self, event):
self._timeline_frame.on_mousewheel(event)
# Zooming
def zoom_in(self):
self._timeline_frame.zoom_in()
def zoom_out(self):
self._timeline_frame.zoom_out()
def reset_zoom(self):
self._timeline_frame.reset_zoom()
def translate_timeline_left(self):
self._timeline_frame.translate_timeline_left()
def translate_timeline_right(self):
self._timeline_frame.translate_timeline_right()
### Publishing
def is_publishing(self, topic):
return self._player and self._player.is_publishing(topic)
def start_publishing(self, topic):
if not self._player and not self._create_player():
return False
self._player.start_publishing(topic)
return True
def stop_publishing(self, topic):
if not self._player:
return False
self._player.stop_publishing(topic)
return True
def _create_player(self):
if not self._player:
try:
self._player = Player(self)
if self._publish_clock:
self._player.start_clock_publishing()
except Exception as ex:
qWarning('Error starting player; aborting publish: %s' % str(ex))
return False
return True
def set_publishing_state(self, start_publishing):
if start_publishing:
for topic in self._timeline_frame.topics:
if not self.start_publishing(topic):
break
else:
for topic in self._timeline_frame.topics:
self.stop_publishing(topic)
# property: play_all
def _get_play_all(self):
return self._play_all
def _set_play_all(self, play_all):
if play_all == self._play_all:
return
self._play_all = not self._play_all
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
play_all = property(_get_play_all, _set_play_all)
def toggle_play_all(self):
self.play_all = not self.play_all
### Playing
def on_idle(self):
self._step_playhead()
def _step_playhead(self):
"""
moves the playhead to the next position based on the desired position
"""
# Reset when the playing mode switchs
if self._timeline_frame.playhead != self.last_playhead:
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
if self._play_all:
self.step_next_message()
else:
self.step_fixed()
def step_fixed(self):
"""
Moves the playhead a fixed distance into the future based on the current play speed
"""
if self.play_speed == 0.0 or not self._timeline_frame.playhead:
self.last_frame = None
self.last_playhead = None
return
now = rospy.Time.from_sec(time.time())
if self.last_frame:
# Get new playhead
if self.stick_to_end:
new_playhead = self.end_stamp
else:
new_playhead = self._timeline_frame.playhead + rospy.Duration.from_sec((now - self.last_frame).to_sec() * self.play_speed)
start_stamp, end_stamp = self._timeline_frame.play_region
if new_playhead > end_stamp:
if self.wrap:
if self.play_speed > 0.0:
new_playhead = start_stamp
else:
new_playhead = end_stamp
else:
new_playhead = end_stamp
if self.play_speed > 0.0:
self.stick_to_end = True
elif new_playhead < start_stamp:
if self.wrap:
if self.play_speed < 0.0:
new_playhead = end_stamp
else:
new_playhead = start_stamp
else:
new_playhead = start_stamp
# Update the playhead
self._timeline_frame.playhead = new_playhead
self.last_frame = now
self.last_playhead = self._timeline_frame.playhead
def step_next_message(self):
"""
Move the playhead to the next message
"""
if self.play_speed <= 0.0 or not self._timeline_frame.playhead:
self.last_frame = None
self.last_playhead = None
return
if self.last_frame:
if not self.desired_playhead:
self.desired_playhead = self._timeline_frame.playhead
else:
delta = rospy.Time.from_sec(time.time()) - self.last_frame
if delta > rospy.Duration.from_sec(0.1):
delta = rospy.Duration.from_sec(0.1)
self.desired_playhead += delta
# Get the occurrence of the next message
next_message_time = self.get_next_message_time()
if next_message_time < self.desired_playhead:
self._timeline_frame.playhead = next_message_time
else:
self._timeline_frame.playhead = self.desired_playhead
self.last_frame = rospy.Time.from_sec(time.time())
self.last_playhead = self._timeline_frame.playhead
### Recording
def record_bag(self, filename, all=True, topics=[], regex=False, limit=0):
try:
self._recorder = Recorder(filename, bag_lock=self._bag_lock, all=all, topics=topics, regex=regex, limit=limit)
except Exception as ex:
qWarning('Error opening bag for recording [%s]: %s' % (filename, str(ex)))
return
self._recorder.add_listener(self._message_recorded)
self.add_bag(self._recorder.bag)
self._recorder.start()
self.wrap = False
self._timeline_frame._index_cache_thread.period = 0.1
self.update()
def toggle_recording(self):
if self._recorder:
self._recorder.toggle_paused()
self.update()
def _message_recorded(self, topic, msg, t):
if self._timeline_frame._start_stamp is None:
self._timeline_frame._start_stamp = t
self._timeline_frame._end_stamp = t
self._timeline_frame._playhead = t
elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp:
self._timeline_frame._end_stamp = t
if not self._timeline_frame.topics or topic not in self._timeline_frame.topics:
self._timeline_frame.topics = self._get_topics()
self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype()
self._playhead_positions_cvs[topic] = threading.Condition()
self._messages_cvs[topic] = threading.Condition()
self._message_loaders[topic] = MessageLoaderThread(self, topic)
if self._timeline_frame._stamp_left is None:
self.reset_zoom()
# Notify the index caching thread that it has work to do
with self._timeline_frame.index_cache_cv:
self._timeline_frame.invalidated_caches.add(topic)
self._timeline_frame.index_cache_cv.notify()
if topic in self._listeners:
for listener in self._listeners[topic]:
try:
listener.timeline_changed()
except Exception as ex:
qWarning('Error calling timeline_changed on %s: %s' % (type(listener), str(ex)))
### Views / listeners
def add_view(self, topic, frame):
self._views.append(frame)
def has_listeners(self, topic):
return topic in self._listeners
def add_listener(self, topic, listener):
self._listeners.setdefault(topic, []).append(listener)
self._message_listener_threads[(topic, listener)] = MessageListenerThread(self, topic, listener)
# Notify the message listeners
self._message_loaders[topic].reset()
with self._playhead_positions_cvs[topic]:
self._playhead_positions_cvs[topic].notify_all()
self.update()
def remove_listener(self, topic, listener):
topic_listeners = self._listeners.get(topic)
if topic_listeners is not None and listener in topic_listeners:
topic_listeners.remove(listener)
if len(topic_listeners) == 0:
del self._listeners[topic]
# Stop the message listener thread
if (topic, listener) in self._message_listener_threads:
self._message_listener_threads[(topic, listener)].stop()
del self._message_listener_threads[(topic, listener)]
self.update()
### Playhead
# property: play_speed
def _get_play_speed(self):
if self._timeline_frame._paused:
return 0.0
return self._play_speed
def _set_play_speed(self, play_speed):
if play_speed == self._play_speed:
return
if play_speed > 0.0:
self._play_speed = min(self._max_play_speed, max(self._min_play_speed, play_speed))
elif play_speed < 0.0:
self._play_speed = max(-self._max_play_speed, min(-self._min_play_speed, play_speed))
else:
self._play_speed = play_speed
if self._play_speed < 1.0:
self.stick_to_end = False
self.update()
play_speed = property(_get_play_speed, _set_play_speed)
def toggle_play(self):
if self._play_speed != 0.0:
self.play_speed = 0.0
else:
self.play_speed = 1.0
def navigate_play(self):
self.play_speed = 1.0
self.last_frame = rospy.Time.from_sec(time.time())
self.last_playhead = self._timeline_frame.playhead
self._play_timer.start()
def navigate_stop(self):
self.play_speed = 0.0
self._play_timer.stop()
def navigate_previous(self):
self.navigate_stop()
self._timeline_frame.playhead = self.get_previous_message_time()
self.last_playhead = self._timeline_frame.playhead
def navigate_next(self):
self.navigate_stop()
self._timeline_frame.playhead = self.get_next_message_time()
self.last_playhead = self._timeline_frame.playhead
def navigate_rewind(self):
if self._play_speed < 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = -1.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
def navigate_fastforward(self):
if self._play_speed > 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = 2.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
def navigate_start(self):
self._timeline_frame.playhead = self._timeline_frame.play_region[0]
def navigate_end(self):
self._timeline_frame.playhead = self._timeline_frame.play_region[1]
class AnimateWorker(QObject):
finished = Signal() # class variable shared by all instances
def __init__(self,
eval_callback,
dt=0.01,
finished_callback=None,
node_name='animate_worker',
frame_id='animation',
parent_frame_id='world',
marker_topic='animation_marker',
new_node=False,
slowdown=1,
parent=None):
super(AnimateWorker, self).__init__(parent)
if new_node:
rospy.init_node(node_name, anonymous=True)
if finished_callback is not None:
self.finished.connect(finished_callback)
#TODO([email protected]) - how to shut down?
self.is_running = True
self.publish = True
self.animate = False
self.eval_callback = eval_callback
self.t = 0.
self.t_max = 0.
self.dt = dt
if slowdown <= 0.001: # prevent speeding up by more than 1000x
slowdown = 1
self.slowdown = slowdown
self.R_old = None
self.frame_id = frame_id
self.parent_frame_id = parent_frame_id
self.tf_br = tf.TransformBroadcaster()
self.marker_pub = rospy.Publisher(marker_topic,
MarkerArray,
queue_size=1)
self.refresh_marker()
print("init animate")
self.timer = QTimer()
self.timer.setInterval(self.dt * 1000 *
self.slowdown) # in milliseconds
#self.timer.setTimerType(Qt.PreciseTimer)
self.timer.timeout.connect(self.on_timer_callback)
self.timer.start()
def refresh_marker(self):
quad_marker = build_quad_marker_template()
quad_marker.id = 0
quad_marker.header.frame_id = self.frame_id
quad_marker.frame_locked = True
quad_marker.action = quad_marker.ADD
quad_marker.pose.orientation.w = 1
quad_marker.pose.position.x = 0.0
quad_marker.pose.position.y = 0.0
quad_marker.pose.position.z = 0.0
quad_marker.lifetime = rospy.Duration(0)
marker_array = MarkerArray()
marker_array.markers.append(quad_marker)
self.marker_pub.publish(marker_array)
def on_timer_callback(self):
if not self.publish:
return
try:
if self.animate:
self.t += self.dt
if self.t > self.t_max:
self.t = 0.
else:
self.t = 0.
(t_max, pos_vec, qw, qx, qy, qz, yaw, vel_vec,
acc_vec) = self.eval_callback(self.t)
self.t_max = t_max
self.tf_br.sendTransform(
(pos_vec[0], pos_vec[1], pos_vec[2]), (qx, qy, qz, qw),
rospy.Time.now(), self.frame_id, self.parent_frame_id)
#except Exception as e:
#print("Error in AnimateWorker")
#print(e)
except:
# print("Unknown error in AnimateWorker")
return
def start_animate(self):
print("Ros helper start animation")
self.animate = True
self.refresh_marker()
def stop_animate(self):
self.animate = False
def start_publish(self):
self.t = 0
self.publish = True
self.refresh_marker()
def stop_publish(self):
self.publish = False
class JointControlPlugin(Plugin):
CM_UPDATE_FREQ = 1
CONTROLLERS = {
'position_controllers/JointPositionController':
JointPositionControllerWidget,
}
def __init__(self, context):
super(JointControlPlugin, self).__init__(context)
self.setObjectName('JointControlPlugin')
self._widget = loadUi('JointControl.ui')
self._widget.setObjectName('JointControlPluginUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.robot_descriptions_list.currentIndexChanged[str].connect(
self._on_robot_descriptions_list_change)
self._widget.cm_list.currentIndexChanged[str].connect(
self._on_cm_change)
self._urdf = None
self._cm_ns = None
self._controllers = []
self._update_robot_descriptions()
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 / self.CM_UPDATE_FREQ)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
self._list_controllers = lambda: []
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
def _update_robot_descriptions(self):
update_combo(self._widget.robot_descriptions_list,
get_robot_description_ns_list())
def _update_cm_list(self):
update_combo(self._widget.cm_list, self._list_cm())
def _on_robot_descriptions_list_change(self, robot_description_ns):
desc = rosparam.get_param(robot_description_ns)
try:
xml = etree.fromstring(desc)
self._urdf = xml.xpath('/robot')[0]
except:
traceback.print_exc()
self._urdf = None
self._update_controllers_list()
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
self._list_controllers = ControllerLister(cm_ns) if cm_ns else (
lambda: [])
self._update_controllers_list()
def _update_controllers_list(self):
controllers = self._list_controllers()
if controllers == self._controllers:
return
for _ in xrange(self._widget.controllers.count()):
self._widget.controllers.takeAt(0)
self._controllers = controllers
joint_state_controller = find(
lambda c: c.type == 'joint_state_controller/JointStateController',
controllers)
for controller in controllers:
self._add_controller(controller)
def _add_controller(self, controller):
if controller.type not in self.CONTROLLERS:
return
widget = self.CONTROLLERS[controller.type](self._cm_ns, controller,
self._urdf)
self._widget.controllers.addWidget(widget)
class JointTrajectoryController(Plugin):
"""
Graphical frontend for a C{JointTrajectoryController}.
There are two modes for interacting with a controller:
1. B{Monitor mode} Joint displays are updated with the state reported
by the controller. This is a read-only mode and does I{not} send
control commands. Every time a new controller is selected, it starts
in monitor mode for safety reasons.
2. B{Control mode} Joint displays update the control command that is
sent to the controller. Commands are sent periodically evan if the
displays are not being updated by the user.
To control the aggressiveness of the motions, the maximum speed of the
sent commands can be scaled down using the C{Speed scaling control}
This plugin is able to detect and keep track of all active controller
managers, as well as the JointTrajectoryControllers that are I{running}
in each controller manager.
For a controller to be compatible with this plugin, it must comply with
the following requisites:
- The controller type contains the C{JointTrajectoryController}
substring, e.g., C{position_controllers/JointTrajectoryController}
- The controller exposes the C{command} and C{state} topics in its
ROS interface.
Additionally, there must be a URDF loaded with a valid joint limit
specification, namely position (when applicable) and velocity limits.
A reference implementation of the C{JointTrajectoryController} is
available in the C{joint_trajectory_controller} ROS package.
"""
_cmd_pub_freq = 10.0 # Hz
_widget_update_freq = 30.0 # Hz
_ctrlrs_update_freq = 1 # Hz
_min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration
jointStateChanged = Signal([dict])
def __init__(self, context):
super(JointTrajectoryController, self).__init__(context)
self.setObjectName('JointTrajectoryController')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'),
'resource',
'joint_trajectory_controller.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JointTrajectoryControllerUi')
# Setup speed scaler
speed_scaling = DoubleEditor(1.0, 100.0)
speed_scaling.spin_box.setSuffix('%')
speed_scaling.spin_box.setValue(50.0)
speed_scaling.spin_box.setDecimals(0)
speed_scaling.setEnabled(False)
self._widget.speed_scaling_layout.addWidget(speed_scaling)
self._speed_scaling_widget = speed_scaling
speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
self._on_speed_scaling_change(speed_scaling.value())
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._jtc_name = [] # Name of selected joint trajectory controller
self._cm_ns = [] # Namespace of the selected controller manager
self._joint_pos = {} # name->pos map for joints of selected controller
self._joint_names = [] # Ordered list of selected controller joints
self._robot_joint_limits = {} # Lazily evaluated on first use
# Timer for sending commands to active controller
self._update_cmd_timer = QTimer(self)
self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
self._update_cmd_timer.timeout.connect(self._update_cmd_cb)
# Timer for updating the joint widgets from the controller state
self._update_act_pos_timer = QTimer(self)
self._update_act_pos_timer.setInterval(1000.0 /
self._widget_update_freq)
self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)
# Timer for controller manager updates
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_jtc_list_timer = QTimer(self)
self._update_jtc_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()
# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber
self._list_controllers = None
def shutdown_plugin(self):
self._update_cmd_timer.stop()
self._update_act_pos_timer.stop()
self._update_cm_list_timer.stop()
self._update_jtc_list_timer.stop()
self._unregister_state_sub()
self._unregister_cmd_pub()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
instance_settings.set_value('jtc_name', self._jtc_name)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
# Resore last session's controller, if running
self._update_jtc_list()
jtc_name = instance_settings.value('jtc_name')
jtc_combo = self._widget.jtc_combo
jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())]
try:
idx = jtc_list.index(jtc_name)
jtc_combo.setCurrentIndex(idx)
except (ValueError):
pass
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
update_combo(self._widget.cm_combo, self._list_cm())
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if not self._list_controllers:
self._widget.jtc_combo.clear()
return
# List of running controllers with a valid joint limits specification
# for _all_ their joints
running_jtc = self._running_jtc_info()
if running_jtc and not self._robot_joint_limits:
self._robot_joint_limits = get_joint_limits() # Lazy evaluation
valid_jtc = []
for jtc_info in running_jtc:
has_limits = all(name in self._robot_joint_limits
for name in _jtc_joint_names(jtc_info))
if has_limits:
valid_jtc.append(jtc_info);
valid_jtc_names = [data.name for data in valid_jtc]
# Update widget
update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))
def _on_speed_scaling_change(self, val):
self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
def _on_joint_state_change(self, actual_pos):
assert(len(actual_pos) == len(self._joint_pos))
for name in actual_pos.keys():
self._joint_pos[name]['position'] = actual_pos[name]
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
if cm_ns:
self._list_controllers = ControllerLister(cm_ns)
# NOTE: Clear below is important, as different controller managers
# might have controllers with the same name but different
# configurations. Clearing forces controller re-discovery
self._widget.jtc_combo.clear()
self._update_jtc_list()
else:
self._list_controllers = None
def _on_jtc_change(self, jtc_name):
self._unload_jtc()
self._jtc_name = jtc_name
if self._jtc_name:
self._load_jtc()
def _on_jtc_enabled(self, val):
# Don't allow enabling if there are no controllers selected
if not self._jtc_name:
self._widget.enable_button.setChecked(False)
return
# Enable/disable joint displays
for joint_widget in self._joint_widgets():
joint_widget.setEnabled(val)
# Enable/disable speed scaling
self._speed_scaling_widget.setEnabled(val)
if val:
# Widgets send desired position commands to controller
self._update_act_pos_timer.stop()
self._update_cmd_timer.start()
else:
# Controller updates widgets with actual position
self._update_cmd_timer.stop()
self._update_act_pos_timer.start()
def _load_jtc(self):
# Initialize joint data corresponding to selected controller
running_jtc = self._running_jtc_info()
self._joint_names = next(_jtc_joint_names(x) for x in running_jtc
if x.name == self._jtc_name)
for name in self._joint_names:
self._joint_pos[name] = {}
# Update joint display
try:
layout = self._widget.joint_group.layout()
for name in self._joint_names:
limits = self._robot_joint_limits[name]
joint_widget = DoubleEditor(limits['min_position'],
limits['max_position'])
layout.addRow(name, joint_widget)
# NOTE: Using partial instead of a lambda because lambdas
# "will not evaluate/look up the argument values before it is
# effectively called, breaking situations like using a loop
# variable inside it"
from functools import partial
par = partial(self._update_single_cmd_cb, name=name)
joint_widget.valueChanged.connect(par)
except:
# TODO: Can we do better than swallow the exception?
from sys import exc_info
print 'Unexpected error:', exc_info()[0]
# Enter monitor mode (sending commands disabled)
self._on_jtc_enabled(False)
# Setup ROS interfaces
jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
state_topic = jtc_ns + '/state'
cmd_topic = jtc_ns + '/command'
self._state_sub = rospy.Subscriber(state_topic,
JointTrajectoryControllerState,
self._state_cb,
queue_size=1)
self._cmd_pub = rospy.Publisher(cmd_topic,
JointTrajectory,
queue_size=1)
# Start updating the joint positions
self.jointStateChanged.connect(self._on_joint_state_change)
def _unload_jtc(self):
# Stop updating the joint positions
try:
self.jointStateChanged.disconnect(self._on_joint_state_change)
except:
pass
# Reset ROS interfaces
self._unregister_state_sub()
self._unregister_cmd_pub()
# Clear joint widgets
# NOTE: Implementation is a workaround for:
# https://bugreports.qt-project.org/browse/QTBUG-15990 :(
layout = self._widget.joint_group.layout()
if layout is not None:
while layout.count():
layout.takeAt(0).widget().deleteLater()
# Delete existing layout by reparenting to temporary
QWidget().setLayout(layout)
self._widget.joint_group.setLayout(QFormLayout())
# Reset joint data
self._joint_names = []
self._joint_pos = {}
# Enforce monitor mode (sending commands disabled)
self._widget.enable_button.setChecked(False)
def _running_jtc_info(self):
from controller_manager_msgs.utils\
import filter_by_type, filter_by_state
controller_list = self._list_controllers()
jtc_list = filter_by_type(controller_list,
'JointTrajectoryController',
match_substring=True)
running_jtc_list = filter_by_state(jtc_list, 'running')
return running_jtc_list
def _unregister_cmd_pub(self):
if self._cmd_pub is not None:
self._cmd_pub.unregister()
self._cmd_pub = None
def _unregister_state_sub(self):
if self._state_sub is not None:
self._state_sub.unregister()
self._state_sub = None
def _state_cb(self, msg):
actual_pos = {}
for i in range(len(msg.joint_names)):
joint_name = msg.joint_names[i]
joint_pos = msg.actual.positions[i]
actual_pos[joint_name] = joint_pos
self.jointStateChanged.emit(actual_pos)
def _update_single_cmd_cb(self, val, name):
self._joint_pos[name]['command'] = val
def _update_cmd_cb(self):
dur = []
traj = JointTrajectory()
traj.joint_names = self._joint_names
point = JointTrajectoryPoint()
for name in traj.joint_names:
pos = self._joint_pos[name]['position']
cmd = pos
try:
cmd = self._joint_pos[name]['command']
except (KeyError):
pass
max_vel = self._robot_joint_limits[name]['max_velocity']
dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
point.positions.append(cmd)
point.time_from_start = rospy.Duration(max(dur) / self._speed_scale)
traj.points.append(point)
self._cmd_pub.publish(traj)
def _update_joint_widgets(self):
joint_widgets = self._joint_widgets()
for id in range(len(joint_widgets)):
joint_name = self._joint_names[id]
try:
joint_pos = self._joint_pos[joint_name]['position']
joint_widgets[id].setValue(joint_pos)
except (KeyError):
pass # Can happen when first connected to controller
def _joint_widgets(self): # TODO: Cache instead of compute every time?
widgets = []
layout = self._widget.joint_group.layout()
for row_id in range(layout.rowCount()):
widgets.append(layout.itemAt(row_id,
QFormLayout.FieldRole).widget())
return widgets
class MotionEditorWidget(QWidget):
position_renamed = Signal(QListWidgetItem)
def __init__(self, motion_publisher, robot_config):
super(MotionEditorWidget, self).__init__()
self.robot_config = robot_config
self._motion_publisher = motion_publisher
self._motion_data = MotionData(robot_config)
self._filter_pattern = ''
self._playback_marker = None
self._playback_timer = None
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'motion_editor.ui')
loadUi(ui_file, self)
self.list_widgets = {}
for group_type in robot_config.group_types():
list_widget = QListWidget()
list_widget.setSortingEnabled(True)
list_widget.setDragDropMode(QAbstractItemView.DragOnly)
list_widget.setContextMenuPolicy(Qt.CustomContextMenu)
list_widget.customContextMenuRequested.connect(
lambda pos, _group_type=group_type: self.positions_list_context_menu(_group_type, pos)
)
list_widget.itemChanged.connect(self.on_list_item_changed)
self.position_lists_layout.addWidget(list_widget)
self.list_widgets[group_type] = list_widget
self._timeline_widget = TimelineWidget()
for track_name in self.robot_config.sorted_groups():
track_type = self.robot_config.groups[track_name].group_type
track = self._timeline_widget.add_track(track_name, track_type)
list_widget = self.list_widgets[track_type]
palette = list_widget.palette()
palette.setColor(QPalette.Base, track._colors['track'])
list_widget.setPalette(palette)
self.timeline_group.layout().addWidget(self._timeline_widget)
for group_type in robot_config.group_types():
label = QLabel(group_type)
label.setAlignment(Qt.AlignHCenter | Qt.AlignVCenter)
self.group_label_layout.addWidget(label)
self.update_motion_name_combo()
self.stop_motion_button.pressed.connect(self.on_motion_stop_pressed)
def on_list_item_changed(self, item):
print 'Position name changed from', item._text, 'to', item.text()
self.position_renamed.emit(item)
def on_motion_stop_pressed(self):
self._clear_playback_marker()
self._motion_publisher.stop_motion()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('splitter', self.splitter.saveState())
instance_settings.set_value('filter_pattern', self.filter_pattern_edit.text())
instance_settings.set_value('filter_motions_checked', self.filter_motions_check.isChecked())
instance_settings.set_value('filter_positions_checked', self.filter_positions_check.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.contains('splitter'):
self.splitter.restoreState(instance_settings.value('splitter'))
else:
self.splitter.setSizes([300, 100])
self.filter_pattern_edit.setText(instance_settings.value('filter_pattern', ''))
self.filter_motions_check.setChecked(instance_settings.value('filter_motions_checked', False) in (1, True, 'true'))
self.filter_positions_check.setChecked(instance_settings.value('filter_positions_checked', False) in (1, True, 'true'))
@Slot()
def on_clear_motion_button_clicked(self):
self._timeline_widget.scene().clear()
@Slot()
def on_delete_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error', 'No motion selected to delete.')
return
if motion_name not in self._motion_data:
QMessageBox.warning(self, 'Error', 'Motion "%s" not in motion list.' % motion_name)
return
self._motion_data.delete(motion_name)
self.update_motion_name_combo()
@Slot()
def on_save_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error', 'No name given to save this motion.')
return
self._motion_data.save(motion_name, self.get_motion_from_timeline())
self.update_motion_name_combo()
@Slot()
def on_load_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error', 'No motion selected to load.')
return
if motion_name not in self._motion_data:
QMessageBox.warning(self, 'Error', 'Motion "%s" not in motion list.' % motion_name)
return
self.on_clear_motion_button_clicked()
motion = self._motion_data[motion_name]
for group_name, poses in motion.items():
for pose in poses:
data = self.robot_config.groups[group_name].adapt_to_side(pose['positions'])
self._timeline_widget.scene().add_clip(group_name, pose['name'], pose['starttime'], pose['duration'], data)
@Slot()
def on_null_motion_button_clicked(self):
self._clear_playback_marker()
for group_name in self.robot_config.groups:
target_position = [0] * len(self.robot_config.groups[group_name].joints)
self._motion_publisher.move_to_position(group_name, target_position, self.time_factor_spin.value())
@Slot()
def on_run_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error', 'No motion selected to run.')
return
if motion_name not in self._motion_data:
QMessageBox.warning(self, 'Error', 'Motion "%s" not in motion list.' % motion_name)
return
self._clear_playback_marker()
self._motion_publisher.publish_motion(self._motion_data[motion_name], self.time_factor_spin.value())
print '[Motion Editor] Running motion:', motion_name
@Slot(str)
def on_filter_pattern_edit_textChanged(self, pattern):
self._filter_pattern = pattern
self._apply_filter_to_position_lists()
self.update_motion_name_combo()
def _apply_filter_to_position_lists(self):
for group_type in self.robot_config.group_types():
list_widget = self.list_widgets[group_type]
for row in range(list_widget.count()):
item = list_widget.item(row)
hidden = self.filter_positions_check.isChecked() and re.search(self._filter_pattern, item.text()) is None
item.setHidden(hidden)
@Slot(bool)
def on_filter_positions_check_toggled(self, checked):
self._apply_filter_to_position_lists()
@Slot(bool)
def on_filter_motions_check_toggled(self, checked):
self.update_motion_name_combo()
def update_motion_name_combo(self):
combo = self.motion_name_combo
# remember selected motion name
motion_name = str(combo.currentText())
# update motion names
combo.clear()
motion_names = self._motion_data.keys()
if self.filter_motions_check.isChecked():
motion_names = [name for name in motion_names if re.search(self._filter_pattern, name) is not None]
combo.addItems(sorted(motion_names))
# restore previously selected motion name
motion_index = combo.findText(motion_name)
combo.setCurrentIndex(motion_index)
def update_positions_lists(self, positions):
for group_type in self.robot_config.group_types():
list_widget = self.list_widgets[group_type]
list_widget.clear()
for name, position in positions[group_type].items():
item = QListWidgetItem(name)
item._data = position
item._text = name
item._type = group_type
item.setFlags(item.flags() | Qt.ItemIsEditable)
list_widget.addItem(item)
self._apply_filter_to_position_lists()
def positions_list_context_menu(self, group_type, pos):
list_widget = self.list_widgets[group_type]
list_item = list_widget.itemAt(pos)
if list_item is None:
return
menu = QMenu()
move_to = {}
for group in self.robot_config.group_list():
if list_item._type == group.group_type:
move_to[menu.addAction('move "%s"' % group.name)] = [group.name]
# move_to[menu.addAction('move all')] = list(move_to.itervalues())
move_to[menu.addAction('move all')] = [group_list[0] for group_list in list(move_to.itervalues())]
result = menu.exec_(list_widget.mapToGlobal(pos))
if result in move_to:
group_names = move_to[result]
for group_name in group_names:
target_positions = self.robot_config.groups[group_name].adapt_to_side(list_item._data)
self._motion_publisher.move_to_position(group_name, target_positions, self.time_factor_spin.value())
print '[Motion Editor] Moving %s to: %s' % (group_name, zip(self.robot_config.groups[group_name].joints_sorted(), target_positions))
def get_motion_from_timeline(self):
motion = {}
for group_name, clips in self._timeline_widget.scene().clips().items():
motion[group_name] = []
for clip in clips:
pose = {
'name': clip.text(),
'starttime': clip.starttime(),
'duration': clip.duration(),
'positions': self.robot_config.groups[group_name].adapt_to_side(clip.data()),
}
motion[group_name].append(pose)
return motion
@Slot()
def on_run_timeline_button_clicked(self):
print '[Motion Editor] Running timeline.'
self._playback_duration = 0.0
for clips in self._timeline_widget.scene().clips().values():
if len(clips) > 0 and self._playback_duration < clips[-1].endtime():
self._playback_duration = clips[-1].endtime()
self._playback_time_factor = self.time_factor_spin.value()
self._clear_playback_marker()
self._playback_marker = self._timeline_widget.scene().add_marker(0.0)
self._playback_timer = QTimer()
self._playback_timer.setInterval(30)
self._playback_timer.timeout.connect(self._playback_update)
self._playback_start = rospy.get_time()
self._motion_publisher.publish_motion(self.get_motion_from_timeline(), self.time_factor_spin.value())
self._playback_timer.start()
def _clear_playback_marker(self):
if self._playback_timer is not None:
self._playback_timer.stop()
if self._playback_marker is not None:
self._playback_marker.remove()
@Slot()
def _playback_update(self):
duration = (rospy.get_time() - self._playback_start) / self._playback_time_factor
if duration > self._playback_duration:
self._clear_playback_marker()
else:
self._playback_marker.set_time(duration)
class JointTrajectoryController(Plugin):
"""
Graphical frontend for a C{JointTrajectoryController}.
There are two modes for interacting with a controller:
1. B{Monitor mode} Joint displays are updated with the state reported
by the controller. This is a read-only mode and does I{not} send
control commands. Every time a new controller is selected, it starts
in monitor mode for safety reasons.
2. B{Control mode} Joint displays update the control command that is
sent to the controller. Commands are sent periodically evan if the
displays are not being updated by the user.
To control the aggressiveness of the motions, the maximum speed of the
sent commands can be scaled down using the C{Speed scaling control}
This plugin is able to detect and keep track of all active controller
managers, as well as the JointTrajectoryControllers that are I{running}
in each controller manager.
For a controller to be compatible with this plugin, it must comply with
the following requisites:
- The controller type contains the C{JointTrajectoryController}
substring, e.g., C{position_controllers/JointTrajectoryController}
- The controller exposes the C{command} and C{state} topics in its
ROS interface.
Additionally, there must be a URDF loaded with a valid joint limit
specification, namely position (when applicable) and velocity limits.
A reference implementation of the C{JointTrajectoryController} is
available in the C{joint_trajectory_controller} ROS package.
"""
_cmd_pub_freq = 10.0 # Hz
_widget_update_freq = 30.0 # Hz
_ctrlrs_update_freq = 1 # Hz
_min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration
jointStateChanged = Signal([dict])
def __init__(self, context):
super(JointTrajectoryController, self).__init__(context)
self.setObjectName('JointTrajectoryController')
self._node = context.node
# Get the robot_description via a topic
qos_profile = QoSProfile(depth=1,
history=HistoryPolicy.KEEP_LAST,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
self._robot_description_sub = self._node.create_subscription(
String, 'robot_description',
self._robot_description_cb, qos_profile)
self._robot_description = None
self._publisher = None
self._widget = QWidget()
_, package_path = get_resource('packages', 'rqt_joint_trajectory_controller')
ui_file = os.path.join(package_path, 'share', 'rqt_joint_trajectory_controller', 'resource', 'joint_trajectory_controller.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JointTrajectoryControllerUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(
self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
ns = self._node.get_namespace()
self._widget.controller_group.setTitle('ns: ' + ns)
# Setup speed scaler
speed_scaling = DoubleEditor(1.0, 100.0)
speed_scaling.spin_box.setSuffix('%')
speed_scaling.spin_box.setValue(50.0)
speed_scaling.spin_box.setDecimals(0)
speed_scaling.setEnabled(False)
self._widget.speed_scaling_layout.addWidget(speed_scaling)
self._speed_scaling_widget = speed_scaling
speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
self._on_speed_scaling_change(speed_scaling.value())
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._jtc_name = [] # Name of selected joint trajectory controller
self._cm_ns = [] # Namespace of the selected controller manager
self._joint_pos = {} # name->pos map for joints of selected controller
self._joint_names = [] # Ordered list of selected controller joints
self._robot_joint_limits = {} # Lazily evaluated on first use
# Timer for sending commands to active controller
self._update_cmd_timer = QTimer(self)
self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
self._update_cmd_timer.timeout.connect(self._update_cmd_cb)
# Timer for updating the joint widgets from the controller state
self._update_act_pos_timer = QTimer(self)
self._update_act_pos_timer.setInterval(1000.0 /
self._widget_update_freq)
self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)
# Timer for controller manager updates
# TODO: self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_jtc_list_timer = QTimer(self)
self._update_jtc_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()
# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber
self._list_controllers = None
self._traj_ns_topic_dict = None
def shutdown_plugin(self):
self._update_cmd_timer.stop()
self._update_act_pos_timer.stop()
self._update_cm_list_timer.stop()
self._update_jtc_list_timer.stop()
self._unregister_state_sub()
self._unregister_cmd_pub()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
instance_settings.set_value('jtc_name', self._jtc_name)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
# Resore last session's controller, if running
self._update_jtc_list()
jtc_name = instance_settings.value('jtc_name')
jtc_combo = self._widget.jtc_combo
jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())]
try:
idx = jtc_list.index(jtc_name)
jtc_combo.setCurrentIndex(idx)
except (ValueError):
pass
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
trajectory_topics = _search_for_trajectory_topics(self._node)
## TODO: remove test code
#trajectory_topics.append('/my_test/controller')
#trajectory_topics.append('/no_namespace')
#trajectory_topics.append('no_root')
self._traj_ns_topic_dict = {}
for full_topic in trajectory_topics:
ns, topic = _split_namespace_from_topic(full_topic)
self._traj_ns_topic_dict.setdefault(ns, []).append(topic)
update_combo(self._widget.cm_combo, list(self._traj_ns_topic_dict.keys()))
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if self._traj_ns_topic_dict is None:
self._widget.jtc_combo.clear()
return
#print(get_joint_limits(self._robot_description))
## List of running controllers with a valid joint limits specification
## for _all_ their joints
#running_jtc = self._running_jtc_info()
#if running_jtc and not self._robot_joint_limits:
# self._robot_joint_limits = get_joint_limits() # Lazy evaluation
#valid_jtc = []
#for jtc_info in running_jtc:
# has_limits = all(name in self._robot_joint_limits
# for name in _jtc_joint_names(jtc_info))
# if has_limits:
# valid_jtc.append(jtc_info);
#valid_jtc_names = [data.name for data in valid_jtc]
# Get the currently selected namespace
curr_ns = self._widget.cm_combo.currentText()
topics = self._traj_ns_topic_dict[curr_ns]
update_combo(self._widget.jtc_combo, topics)
def _on_speed_scaling_change(self, val):
self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
def _on_joint_state_change(self, actual_pos):
assert(len(actual_pos) == len(self._joint_pos))
for name in actual_pos.keys():
self._joint_pos[name]['position'] = actual_pos[name]
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
if cm_ns:
self._widget.jtc_combo.clear()
self._update_jtc_list()
else:
self._list_controllers = None
def _on_jtc_change(self, jtc_name):
self._unload_jtc()
self._jtc_name = jtc_name
if self._jtc_name:
self._load_jtc()
def _on_jtc_enabled(self, val):
# Don't allow enabling if there are no controllers selected
if not self._jtc_name:
self._widget.enable_button.setChecked(False)
return
# Enable/disable joint displays
for joint_widget in self._joint_widgets():
joint_widget.setEnabled(val)
# Enable/disable speed scaling
self._speed_scaling_widget.setEnabled(val)
if val:
# Widgets send desired position commands to controller
self._update_act_pos_timer.stop()
self._update_cmd_timer.start()
else:
# Controller updates widgets with actual position
self._update_cmd_timer.stop()
self._update_act_pos_timer.start()
def _load_jtc(self):
self._robot_joint_limits = get_joint_limits(self._robot_description)
self._joint_names = list(self._robot_joint_limits.keys())
self._joint_pos = { name: {} for name in self._joint_names }
# Update joint display
try:
layout = self._widget.joint_group.layout()
for name in self._joint_names:
limits = self._robot_joint_limits[name]
joint_widget = DoubleEditor(limits['min_position'],
limits['max_position'])
layout.addRow(name, joint_widget)
# NOTE: Using partial instead of a lambda because lambdas
# "will not evaluate/look up the argument values before it is
# effectively called, breaking situations like using a loop
# variable inside it"
from functools import partial
par = partial(self._update_single_cmd_cb, name=name)
joint_widget.valueChanged.connect(par)
except:
# TODO: Can we do better than swallow the exception?
from sys import exc_info
print('Unexpected error:', exc_info()[0])
# Enter monitor mode (sending commands disabled)
self._on_jtc_enabled(False)
# Setup ROS interfaces
jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
#state_topic = jtc_ns + '/state'
state_topic = 'state'
#cmd_topic = jtc_ns + '/command'
cmd_topic = '/joint_trajectory_controller/joint_trajectory'
#self._state_sub = rospy.Subscriber(state_topic,
# JointTrajectoryControllerState,
# self._state_cb,
# queue_size=1)
qos_profile = QoSProfile(depth=1,
history=HistoryPolicy.KEEP_LAST,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
self._state_sub = self._node.create_subscription(
JointTrajectoryControllerState, state_topic, self._state_cb,
qos_profile)
self._cmd_pub = self._node.create_publisher(JointTrajectory, cmd_topic, 1)
# Start updating the joint positions
self.jointStateChanged.connect(self._on_joint_state_change)
def _unload_jtc(self):
# Stop updating the joint positions
try:
self.jointStateChanged.disconnect(self._on_joint_state_change)
except:
pass
# Reset ROS interfaces
self._unregister_state_sub()
self._unregister_cmd_pub()
# Clear joint widgets
# NOTE: Implementation is a workaround for:
# https://bugreports.qt-project.org/browse/QTBUG-15990 :(
layout = self._widget.joint_group.layout()
if layout is not None:
while layout.count():
layout.takeAt(0).widget().deleteLater()
# Delete existing layout by reparenting to temporary
QWidget().setLayout(layout)
self._widget.joint_group.setLayout(QFormLayout())
# Reset joint data
self._joint_names = []
self._joint_pos = {}
# Enforce monitor mode (sending commands disabled)
self._widget.enable_button.setChecked(False)
def _running_jtc_info(self):
from controller_manager_msgs.utils\
import filter_by_type, filter_by_state
controller_list = self._list_controllers()
jtc_list = filter_by_type(controller_list,
'JointTrajectoryController',
match_substring=True)
running_jtc_list = filter_by_state(jtc_list, 'running')
return running_jtc_list
def _unregister_cmd_pub(self):
if self._cmd_pub is not None:
self._node.destroy_publisher(self._cmd_pub)
self._cmd_pub = None
def _unregister_state_sub(self):
if self._state_sub is not None:
self._node.destroy_subscription(self._state_sub)
self._state_sub = None
def _robot_description_cb(self, msg):
self._robot_description = msg.data
def _state_cb(self, msg):
actual_pos = {}
for i in range(len(msg.joint_names)):
joint_name = msg.joint_names[i]
joint_pos = msg.actual.positions[i]
actual_pos[joint_name] = joint_pos
self.jointStateChanged.emit(actual_pos)
def _update_single_cmd_cb(self, val, name):
self._joint_pos[name]['command'] = val
def _update_cmd_cb(self):
dur = []
traj = JointTrajectory()
traj.joint_names = self._joint_names
point = JointTrajectoryPoint()
for name in traj.joint_names:
pos = self._joint_pos[name]['position']
cmd = pos
try:
cmd = self._joint_pos[name]['command']
except (KeyError):
pass
max_vel = self._robot_joint_limits[name]['max_velocity']
dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
point.positions.append(cmd)
max_duration_scaled = max(dur) / self._speed_scale
seconds = floor(max_duration_scaled)
nanoseconds = (max_duration_scaled - seconds) * 1e9
duration = Duration(seconds=seconds, nanoseconds=nanoseconds)
point.time_from_start = duration.to_msg()
traj.points.append(point)
self._cmd_pub.publish(traj)
def _update_joint_widgets(self):
joint_widgets = self._joint_widgets()
for id in range(len(joint_widgets)):
joint_name = self._joint_names[id]
try:
joint_pos = self._joint_pos[joint_name]['position']
joint_widgets[id].setValue(joint_pos)
except (KeyError):
pass # Can happen when first connected to controller
def _joint_widgets(self): # TODO: Cache instead of compute every time?
widgets = []
layout = self._widget.joint_group.layout()
for row_id in range(layout.rowCount()):
widgets.append(layout.itemAt(row_id,
QFormLayout.FieldRole).widget())
return widgets
class ControllerManager(Plugin):
"""
Graphical frontend for managing ros_control controllers.
"""
_cm_update_freq = 1 # Hz
def __init__(self, context):
super(ControllerManager, self).__init__(context)
self.setObjectName('ControllerManager')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_controller_manager'),
'resource',
'controller_manager.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('ControllerManagerUi')
# Pop-up that displays controller information
self._popup_widget = QWidget()
ui_file = os.path.join(rp.get_path('rqt_controller_manager'),
'resource',
'controller_info.ui')
loadUi(ui_file, self._popup_widget)
self._popup_widget.setObjectName('ControllerInfoUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._cm_ns = [] # Namespace of the selected controller manager
self._controllers = [] # State of each controller
self._table_model = None
self._controller_lister = None
# Controller manager service proxies
self._load_srv = None
self._unload_srv = None
self._switch_srv = None
# Controller state icons
rospack = rospkg.RosPack()
path = rospack.get_path('rqt_controller_manager')
self._icons = {'running': QIcon(path + '/resource/led_green.png'),
'stopped': QIcon(path + '/resource/led_red.png'),
'uninitialized': QIcon(path + '/resource/led_off.png')}
# Controllers display
table_view = self._widget.table_view
table_view.setContextMenuPolicy(Qt.CustomContextMenu)
table_view.customContextMenuRequested.connect(self._on_ctrl_menu)
table_view.doubleClicked.connect(self._on_ctrl_info)
header = table_view.horizontalHeader()
header.setSectionResizeMode(QHeaderView.ResizeToContents)
header.setContextMenuPolicy(Qt.CustomContextMenu)
header.customContextMenuRequested.connect(self._on_header_menu)
# Timer for controller manager updates
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._cm_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_ctrl_list_timer = QTimer(self)
self._update_ctrl_list_timer.setInterval(1000.0 /
self._cm_update_freq)
self._update_ctrl_list_timer.timeout.connect(self._update_controllers)
self._update_ctrl_list_timer.start()
# Signal connections
w = self._widget
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
def shutdown_plugin(self):
self._update_cm_list_timer.stop()
self._update_ctrl_list_timer.stop()
self._popup_widget.hide()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
update_combo(self._widget.cm_combo, self._list_cm())
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
# Setup services for communicating with the selected controller manager
self._set_cm_services(cm_ns)
# Controller lister for the selected controller manager
if cm_ns:
self._controller_lister = ControllerLister(cm_ns)
self._update_controllers()
else:
self._controller_lister = None
def _set_cm_services(self, cm_ns):
if cm_ns:
# NOTE: Persistent services are used for performance reasons.
# If the controller manager dies, we detect it and disconnect from
# it anyway
load_srv_name = _append_ns(cm_ns, 'load_controller')
self._load_srv = rospy.ServiceProxy(load_srv_name,
LoadController,
persistent=True)
unload_srv_name = _append_ns(cm_ns, 'unload_controller')
self._unload_srv = rospy.ServiceProxy(unload_srv_name,
UnloadController,
persistent=True)
switch_srv_name = _append_ns(cm_ns, 'switch_controller')
self._switch_srv = rospy.ServiceProxy(switch_srv_name,
SwitchController,
persistent=True)
else:
self._load_srv = None
self._unload_srv = None
self._switch_srv = None
def _update_controllers(self):
# Find controllers associated to the selected controller manager
controllers = self._list_controllers()
# Update controller display, if necessary
if self._controllers != controllers:
self._controllers = controllers
self._show_controllers() # NOTE: Model is recomputed from scratch
def _list_controllers(self):
"""
@return List of controllers associated to a controller manager
namespace. Contains both stopped/running controllers, as returned by
the C{list_controllers} service, plus uninitialized controllers with
configurations loaded in the parameter server.
@rtype [str]
"""
if not self._cm_ns:
return []
# Add loaded controllers first
controllers = self._controller_lister()
# Append potential controller configs found in the parameter server
all_ctrls_ns = _resolve_controllers_ns(self._cm_ns)
for name in get_rosparam_controller_names(all_ctrls_ns):
add_ctrl = not any(name == ctrl.name for ctrl in controllers)
if add_ctrl:
type_str = _rosparam_controller_type(all_ctrls_ns, name)
uninit_ctrl = ControllerState(name=name,
type=type_str,
state='uninitialized')
controllers.append(uninit_ctrl)
return controllers
def _show_controllers(self):
table_view = self._widget.table_view
self._table_model = ControllerTable(self._controllers, self._icons)
table_view.setModel(self._table_model)
def _on_ctrl_menu(self, pos):
# Get data of selected controller
row = self._widget.table_view.rowAt(pos.y())
if row < 0:
return # Cursor is not under a valid item
ctrl = self._controllers[row]
# Show context menu
menu = QMenu(self._widget.table_view)
if ctrl.state == 'running':
action_stop = menu.addAction(self._icons['stopped'], 'Stop')
action_kill = menu.addAction(self._icons['uninitialized'],
'Stop and Unload')
elif ctrl.state == 'stopped':
action_start = menu.addAction(self._icons['running'], 'Start')
action_unload = menu.addAction(self._icons['uninitialized'],
'Unload')
elif ctrl.state == 'uninitialized':
action_load = menu.addAction(self._icons['stopped'], 'Load')
action_spawn = menu.addAction(self._icons['running'],
'Load and Start')
action = menu.exec_(self._widget.table_view.mapToGlobal(pos))
# Evaluate user action
if ctrl.state == 'running':
if action is action_stop:
self._stop_controller(ctrl.name)
elif action is action_kill:
self._stop_controller(ctrl.name)
self._unload_controller(ctrl.name)
elif ctrl.state == 'stopped':
if action is action_start:
self._start_controller(ctrl.name)
elif action is action_unload:
self._unload_controller(ctrl.name)
elif ctrl.state == 'uninitialized':
if action is action_load:
self._load_controller(ctrl.name)
if action is action_spawn:
self._load_controller(ctrl.name)
self._start_controller(ctrl.name)
def _on_ctrl_info(self, index):
popup = self._popup_widget
ctrl = self._controllers[index.row()]
popup.ctrl_name.setText(ctrl.name)
popup.ctrl_type.setText(ctrl.type)
res_model = QStandardItemModel()
model_root = QStandardItem('Claimed Resources')
res_model.appendRow(model_root)
for hw_res in ctrl.claimed_resources:
hw_iface_item = QStandardItem(hw_res.hardware_interface)
model_root.appendRow(hw_iface_item)
for res in hw_res.resources:
res_item = QStandardItem(res)
hw_iface_item.appendRow(res_item)
popup.resource_tree.setModel(res_model)
popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree))
popup.resource_tree.expandAll()
popup.move(QCursor.pos())
popup.show()
def _on_header_menu(self, pos):
header = self._widget.table_view.horizontalHeader()
# Show context menu
menu = QMenu(self._widget.table_view)
action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize')
action = menu.exec_(header.mapToGlobal(pos))
# Evaluate user action
if action is action_toggle_auto_resize:
if header.resizeMode(0) == QHeaderView.ResizeToContents:
header.setSectionResizeMode(QHeaderView.Interactive)
else:
header.setSectionResizeMode(QHeaderView.ResizeToContents)
def _load_controller(self, name):
self._load_srv.call(LoadControllerRequest(name=name))
def _unload_controller(self, name):
self._unload_srv.call(UnloadControllerRequest(name=name))
def _start_controller(self, name):
strict = SwitchControllerRequest.STRICT
req = SwitchControllerRequest(start_controllers=[name],
stop_controllers=[],
strictness=strict)
self._switch_srv.call(req)
def _stop_controller(self, name):
strict = SwitchControllerRequest.STRICT
req = SwitchControllerRequest(start_controllers=[],
stop_controllers=[name],
strictness=strict)
self._switch_srv.call(req)
class LifeFrame(QFrame):
STATE_STOPPED = "stopped"
STATE_RUN = "running"
STATE_IDLE = "idle"
def __init__(self, parent=None):
super(LifeFrame, self).__init__(parent)
self._ui = Ui_life_frame()
self._motion = Rotate('/mobile_base/commands/velocity')
self._motion_thread = None
self._timer = QTimer()
#self._timer.setInterval(60000) #60s
self._timer.setInterval(250) #60s
QObject.connect(self._timer, SIGNAL('timeout()'), self,
SLOT('update_progress_callback()'))
self._state = LifeFrame.STATE_STOPPED
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.angular_speed_spinbox.value())
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: life frame shutdown")
self._motion.shutdown()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def start(self):
self._state = LifeFrame.STATE_RUN
self._ui.run_progress.reset()
self._ui.idle_progress.reset()
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
def stop(self):
self._state = LifeFrame.STATE_STOPPED
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._timer.start()
self.start()
@Slot()
def on_stop_button_clicked(self):
self.stop()
self._timer.stop()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.angular_speed_spinbox.value())
##########################################################################
# Timer Callbacks
##########################################################################
@Slot()
def update_progress_callback(self):
if self._state == LifeFrame.STATE_RUN:
new_value = self._ui.run_progress.value() + 1
if new_value == self._ui.run_progress.maximum():
print(" Switching to idle")
self._motion.stop()
self._state = LifeFrame.STATE_IDLE
else:
self._ui.run_progress.setValue(new_value)
if self._state == LifeFrame.STATE_IDLE:
new_value = self._ui.idle_progress.value() + 1
if new_value == self._ui.idle_progress.maximum():
print(" Switching to run")
self.start()
else:
self._ui.idle_progress.setValue(new_value)
class JointTrajectoryController(Plugin):
"""
Graphical frontend for a C{JointTrajectoryController}.
There are two modes for interacting with a controller:
1. B{Monitor mode} Joint displays are updated with the state reported
by the controller. This is a read-only mode and does I{not} send
control commands. Every time a new controller is selected, it starts
in monitor mode for safety reasons.
2. B{Control mode} Joint displays update the control command that is
sent to the controller. Commands are sent periodically evan if the
displays are not being updated by the user.
To control the aggressiveness of the motions, the maximum speed of the
sent commands can be scaled down using the C{Speed scaling control}
This plugin is able to detect and keep track of all active controller
managers, as well as the JointTrajectoryControllers that are I{running}
in each controller manager.
For a controller to be compatible with this plugin, it must comply with
the following requisites:
- The controller type contains the C{JointTrajectoryController}
substring, e.g., C{position_controllers/JointTrajectoryController}
- The controller exposes the C{command} and C{state} topics in its
ROS interface.
Additionally, there must be a URDF loaded with a valid joint limit
specification, namely position (when applicable) and velocity limits.
A reference implementation of the C{JointTrajectoryController} is
available in the C{joint_trajectory_controller} ROS package.
"""
_cmd_pub_freq = 10.0 # Hz
_widget_update_freq = 30.0 # Hz
_ctrlrs_update_freq = 1 # Hz
_min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration
jointStateChanged = Signal([dict])
def __init__(self, context):
super(JointTrajectoryController, self).__init__(context)
self.setObjectName('JointTrajectoryController')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'),
'resource', 'joint_trajectory_controller.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JointTrajectoryControllerUi')
# Setup speed scaler
speed_scaling = DoubleEditor(1.0, 100.0)
speed_scaling.spin_box.setSuffix('%')
speed_scaling.spin_box.setValue(50.0)
speed_scaling.spin_box.setDecimals(0)
speed_scaling.setEnabled(False)
self._widget.speed_scaling_layout.addWidget(speed_scaling)
self._speed_scaling_widget = speed_scaling
speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
self._on_speed_scaling_change(speed_scaling.value())
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._jtc_name = [] # Name of selected joint trajectory controller
self._cm_ns = [] # Namespace of the selected controller manager
self._joint_pos = {} # name->pos map for joints of selected controller
self._joint_names = [] # Ordered list of selected controller joints
self._robot_joint_limits = get_joint_limits() # For all robot joints
# Timer for sending commands to active controller
self._update_cmd_timer = QTimer(self)
self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
self._update_cmd_timer.timeout.connect(self._update_cmd_cb)
# Timer for updating the joint widgets from the controller state
self._update_act_pos_timer = QTimer(self)
self._update_act_pos_timer.setInterval(1000.0 /
self._widget_update_freq)
self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)
# Timer for controller manager updates
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_jtc_list_timer = QTimer(self)
self._update_jtc_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()
# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber
self._list_controllers = None
def shutdown_plugin(self):
self._update_cmd_timer.stop()
self._update_act_pos_timer.stop()
self._update_cm_list_timer.stop()
self._update_jtc_list_timer.stop()
self._unregister_state_sub()
self._unregister_cmd_pub()
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
update_combo(self._widget.cm_combo, self._list_cm())
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if not self._list_controllers:
self._widget.jtc_combo.clear()
return
# Update widget
running_jtc = self._running_jtc_info()
running_jtc_names = [data.name for data in running_jtc]
update_combo(self._widget.jtc_combo, sorted(running_jtc_names))
def _on_speed_scaling_change(self, val):
self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
def _on_joint_state_change(self, actual_pos):
assert (len(actual_pos) == len(self._joint_pos))
for name in actual_pos.keys():
self._joint_pos[name]['position'] = actual_pos[name]
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
if cm_ns:
self._list_controllers = ControllerLister(cm_ns)
self._update_jtc_list()
else:
self._list_controllers = None
def _on_jtc_change(self, jtc_name):
self._unload_jtc()
self._jtc_name = jtc_name
if self._jtc_name:
self._load_jtc()
def _on_jtc_enabled(self, val):
# Don't allow enabling if there are no controllers selected
if not self._jtc_name:
self._widget.enable_button.setChecked(False)
return
# Enable/disable joint displays
for joint_widget in self._joint_widgets():
joint_widget.setEnabled(val)
# Enable/disable speed scaling
self._speed_scaling_widget.setEnabled(val)
if val:
# Widgets send desired position commands to controller
self._update_act_pos_timer.stop()
self._update_cmd_timer.start()
else:
# Controller updates widgets with actual position
self._update_cmd_timer.stop()
self._update_act_pos_timer.start()
def _load_jtc(self):
# Initialize joint data corresponding to selected controller
running_jtc = self._running_jtc_info()
self._joint_names = next(x.resources for x in running_jtc
if x.name == self._jtc_name)
for name in self._joint_names:
self._joint_pos[name] = {}
# Update joint display
try:
layout = self._widget.joint_group.layout()
for name in self._joint_names:
limits = self._robot_joint_limits[name]
joint_widget = DoubleEditor(limits['min_position'],
limits['max_position'])
layout.addRow(name, joint_widget)
# NOTE: Using partial instead of a lambda because lambdas
# "will not evaluate/look up the argument values before it is
# effectively called, breaking situations like using a loop
# variable inside it"
from functools import partial
par = partial(self._update_single_cmd_cb, name=name)
joint_widget.valueChanged.connect(par)
except:
# TODO: Can we do better than swallow the exception?
from sys import exc_info
print 'Unexpected error:', exc_info()[0]
# Enter monitor mode (sending commands disabled)
self._on_jtc_enabled(False)
# Setup ROS interfaces
jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
state_topic = jtc_ns + '/state'
cmd_topic = jtc_ns + '/command'
self._state_sub = rospy.Subscriber(state_topic,
JointTrajectoryControllerState,
self._state_cb,
queue_size=1)
self._cmd_pub = rospy.Publisher(cmd_topic,
JointTrajectory,
queue_size=1)
# Start updating the joint positions
self.jointStateChanged.connect(self._on_joint_state_change)
def _unload_jtc(self):
# Stop updating the joint positions
try:
self.jointStateChanged.disconnect(self._on_joint_state_change)
except:
pass
# Reset ROS interfaces
self._unregister_state_sub()
self._unregister_cmd_pub()
# Clear joint widgets
# NOTE: Implementation is a workaround for:
# https://bugreports.qt-project.org/browse/QTBUG-15990 :(
layout = self._widget.joint_group.layout()
if layout is not None:
while layout.count():
layout.takeAt(0).widget().deleteLater()
# Delete existing layout by reparenting to temporary
QWidget().setLayout(layout)
self._widget.joint_group.setLayout(QFormLayout())
# Reset joint data
self._joint_names = []
self._joint_pos = {}
# Enforce monitor mode (sending commands disabled)
self._widget.enable_button.setChecked(False)
def _running_jtc_info(self):
from controller_manager_msgs.utils\
import filter_by_type, filter_by_state
controller_list = self._list_controllers()
jtc_list = filter_by_type(controller_list,
'JointTrajectoryController',
match_substring=True)
running_jtc_list = filter_by_state(jtc_list, 'running')
return running_jtc_list
def _unregister_cmd_pub(self):
if self._cmd_pub is not None:
self._cmd_pub.unregister()
self._cmd_pub = None
def _unregister_state_sub(self):
if self._state_sub is not None:
self._state_sub.unregister()
self._state_sub = None
def _state_cb(self, msg):
actual_pos = {}
for i in range(len(msg.joint_names)):
joint_name = msg.joint_names[i]
joint_pos = msg.actual.positions[i]
actual_pos[joint_name] = joint_pos
self.jointStateChanged.emit(actual_pos)
def _update_single_cmd_cb(self, val, name):
self._joint_pos[name]['command'] = val
def _update_cmd_cb(self):
dur = []
traj = JointTrajectory()
traj.joint_names = self._joint_names
point = JointTrajectoryPoint()
for name in traj.joint_names:
pos = self._joint_pos[name]['position']
cmd = pos
try:
cmd = self._joint_pos[name]['command']
except (KeyError):
pass
max_vel = self._robot_joint_limits[name]['max_velocity']
dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
point.positions.append(cmd)
point.time_from_start = rospy.Duration(max(dur) / self._speed_scale)
traj.points.append(point)
self._cmd_pub.publish(traj)
def _update_joint_widgets(self):
joint_widgets = self._joint_widgets()
for id in range(len(joint_widgets)):
joint_name = self._joint_names[id]
try:
joint_pos = self._joint_pos[joint_name]['position']
joint_widgets[id].setValue(joint_pos)
except (KeyError):
pass # Can happen when first connected to controller
def _joint_widgets(self): # TODO: Cache instead of compute every time?
widgets = []
layout = self._widget.joint_group.layout()
for row_id in range(layout.rowCount()):
widgets.append(
layout.itemAt(row_id, QFormLayout.FieldRole).widget())
return widgets
