class SettingsChannel(ChannelInterface):
yaml_config_signal = Signal(str, str)
'''
:ivar str,str yaml_config_signal: signal emit YAML configuration from daemon {YAML string, grpc_url}.
'''
def clear_cache(self, grpc_url=''):
pass
def get_settings_manager(self, uri='localhost:12321'):
channel = remote.get_insecure_channel(uri)
if channel is not None:
return scstub.SettingsStub(channel)
raise Exception("Node manager daemon '%s' not reachable" % uri)
def get_config_threaded(self, grpc_url='grpc://localhost:12321'):
self._threads.start_thread("gcfgt_%s" % grpc_url,
target=self.get_config,
args=(grpc_url, True))
def get_config(self, grpc_url='grpc://localhost:12321', threaded=False):
rospy.logdebug("get config from %s" % (grpc_url))
uri, _ = nmdurl.split(grpc_url)
sm = self.get_settings_manager(uri)
try:
yaml_cfg = sm.get_config()
if threaded:
self.yaml_config_signal.emit(yaml_cfg, grpc_url)
self._threads.finished("gcfgt_%s" % grpc_url)
return yaml_cfg
except Exception as e:
self.error.emit("get_config", "grpc://%s" % uri, "", e)
def set_config(self, grpc_url='grpc://localhost:12321', data=''):
rospy.logdebug("set config to %s" % (grpc_url))
uri, _ = nmdurl.split(grpc_url)
sm = self.get_settings_manager(uri)
sm.set_config(data)
class RequestBinariesThread(QObject, threading.Thread):
'''
A thread to to retrieve the binaries for a given package
and publish it by sending a QT signal.
'''
binaries_signal = Signal(str, dict)
def __init__(self, package, grpc_url, parent=None):
QObject.__init__(self)
threading.Thread.__init__(self)
self._grpc_url = grpc_url
self._package = package
self.setDaemon(True)
self._canceled = False
self._result = {}
def cancel(self):
self._canceled = True
@property
def pkgname(self):
return self._package
def reemit(self):
self.binaries_signal.emit(self._package, self._result)
def run(self):
'''
'''
if self._grpc_url:
try:
self._result = nm.nmd().file.get_package_binaries(
self._package, nmdurl.nmduri(self._grpc_url))
if not self._canceled:
self.binaries_signal.emit(self._package, self._result)
except Exception:
import traceback
print(traceback.format_exc())
class PackagesThread(QObject, threading.Thread):
'''
A thread to list all available ROS packages and
publish there be sending a QT signal.
'''
packages = Signal(dict)
'''
@ivar: packages is a signal, which is emitted, if a list with ROS packages was
created {package : path}.
'''
def __init__(self):
'''
'''
QObject.__init__(self)
threading.Thread.__init__(self)
self.setDaemon(True)
def run(self):
'''
'''
try:
# fill the input fields
root_paths = [os.path.normpath(p) for p in os.getenv("ROS_PACKAGE_PATH").split(':')]
packages = {}
for p in root_paths:
ret = get_packages(p)
packages = dict(ret.items() + packages.items())
self.packages.emit(packages)
except:
import traceback
formatted_lines = traceback.format_exc(1).splitlines()
print "Error while list packages:\n\t%s" % traceback.format_exc()
try:
rospy.logwarn("Error while list packages:\n\t%s", formatted_lines[-1])
except:
pass
class RequestListThread(QObject, threading.Thread):
'''
A thread to to retrieve the parameter list from ROSparameter server
and publish it by sending a QT signal.
'''
parameter_list_signal = Signal(str, int, str, list)
def __init__(self, masteruri, ns, parent=None):
QObject.__init__(self)
threading.Thread.__init__(self)
self._masteruri = masteruri
self._ns = ns
self.setDaemon(True)
def run(self):
'''
'''
if self._masteruri:
try:
name = rospy.get_name()
master = xmlrpclib.ServerProxy(self._masteruri)
code, msg, params = master.getParamNames(name)
# filter the parameter
result = []
for p in params:
if p.startswith(self._ns):
result.append(p)
self.parameter_list_signal.emit(self._masteruri, code, msg,
result)
except Exception:
import traceback
err_msg = "Error while retrieve the parameter list from %s: %s" % (
self._masteruri, traceback.format_exc(1))
rospy.logwarn(err_msg)
# lines = traceback.format_exc(1).splitlines()
self.parameter_list_signal.emit(self._masteruri, -1, err_msg,
[])
class RequestValuesThread(QObject, threading.Thread):
'''
A thread to to retrieve the value for given parameter from ROSparameter server
and publish it by sending a QT signal.
'''
parameter_values_signal = Signal(str, int, str, dict)
def __init__(self, masteruri, params, parent=None):
QObject.__init__(self)
threading.Thread.__init__(self)
self._masteruri = masteruri
self._params = params
self.setDaemon(True)
def run(self):
'''
'''
if self._masteruri:
result = dict()
for p in self._params:
result[p] = None
try:
name = rospy.get_name()
master = xmlrpcclient.ServerProxy(self._masteruri)
param_server_multi = xmlrpcclient.MultiCall(master)
for p in self._params:
param_server_multi.getParam(name, p)
r = param_server_multi()
for index, (code, msg, value) in enumerate(r):
result[self._params[index]] = (code, msg, value)
self.parameter_values_signal.emit(self._masteruri, 1, '', result)
except Exception:
import traceback
# err_msg = "Error while retrieve parameter values from %s: %s"%(self._masteruri, traceback.format_exc(1))
# rospy.logwarn(err_msg)
# lines = traceback.format_exc(1).splitlines()
self.parameter_values_signal.emit(self._masteruri, -1, traceback.format_exc(1), result)
class StatusSnapshot(QTextEdit):
"""Display a single static status message. Helps facilitate copy/paste"""
write_status = Signal(DiagnosticStatus)
def __init__(self, status=None, parent=None):
super(StatusSnapshot, self).__init__()
self.write_status.connect(self._write_status)
if status is not None:
self.write_status.emit(status)
self.resize(300, 400)
self.show()
def _write_status(self, status):
self.clear()
self._write("Full Name", status.name)
self._write("Component", status.name.split('/')[-1])
self._write("Hardware ID", status.hardware_id)
self._write("Level", level_to_text(status.level))
self._write("Message", status.message)
self.insertPlainText('\n')
for value in status.values:
self._write(value.key, value.value)
def _write(self, k, v):
# TODO(ahendrix): write these as a table rather than as text
self.setFontWeight(75)
self.insertPlainText(str(k))
# TODO(ahendrix): de-dupe trailing ':' here
self.insertPlainText(': ')
self.setFontWeight(50)
self.insertPlainText(str(v))
self.insertPlainText('\n')
class RobotModeTextWidget(QTextEdit):
text_changed = Signal()
def __init__(self):
super(QTextEdit, self).__init__()
self.setObjectName("robot_mode_text")
self.setReadOnly(True)
self.setText('STALE')
self.setFixedSize(QSize(180, 27))
self.text_changed.connect(self._update_state)
self.__state = 'STALE'
def update_state(self, state):
if type(state) is str:
self.__state = state
self.text_changed.emit()
else:
raise TypeError('state must be a string')
def _update_state(self):
self.setText(self.__state)
def state(self):
return self.__state
class Logger(QObject):
out_log_signal = Signal(str, QColor)
def __init__(self, error_status_text_box=None):
super(Logger, self).__init__()
self.connect(error_status_text_box)
def connect(self, error_status_text_box):
if type(error_status_text_box).__name__ == "QErrorStatusTextBox":
self.out_log_signal.connect(error_status_text_box.out_log)
elif type(error_status_text_box).__name__ == "QErrorStatusWidget":
self.out_log_signal.connect(
error_status_text_box.get_text_box().out_log)
def log(self, error_status):
if error_status.error != 0:
self.log_error(error_status.error_msg)
if error_status.warning != 0:
self.log_warn(error_status.warning_msg)
def log_info(self, msg):
self.out_log_signal.emit("[ INFO] " + msg, Qt.black)
rospy.loginfo(msg)
def log_debug(self, msg):
self.out_log_signal.emit("[DEBUG] " + msg, Qt.yellow)
rospy.logdebug(msg)
def log_warn(self, msg):
self.out_log_signal.emit("[ WARN] " + msg, QColor(255, 165, 0))
rospy.logwarn(msg)
def log_error(self, msg):
self.out_log_signal.emit("[ERROR] " + msg, Qt.red)
rospy.logerr(msg)
class RobotMonitorWidget(QWidget):
"""
NOTE: RobotMonitorWidget.shutdown function needs to be called
when the instance of this class terminates.
RobotMonitorWidget itself doesn't store previous diagnostic states.
It instead delegates that function to Timeline class.
"""
_TREE_ALL = 1
_TREE_WARN = 2
_TREE_ERR = 3
message_updated = Signal(DiagnosticArray)
def __init__(self, context, topic=None):
"""
:param context: plugin context hook to enable adding widgets as a
ROS_GUI pane, 'PluginContext'
:param topic: Diagnostic topic to subscribe to 'str'
"""
super(RobotMonitorWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_monitor'), 'resource',
'robotmonitor_mainwidget.ui')
loadUi(ui_file, self)
obj_name = 'Robot Monitor'
self.setObjectName(obj_name)
self.setWindowTitle(obj_name)
self.message_updated.connect(self.message_cb)
# if we're given a topic, create a timeline. otherwise, don't
# this can be used later when writing an rqt_bag plugin
if topic:
# create timeline data structure
self._timeline = Timeline(topic, DiagnosticArray)
self._timeline.message_updated.connect(self.message_updated)
# create timeline pane widget
self.timeline_pane = TimelinePane(self)
self.timeline_pane.set_timeline(self._timeline)
self.vlayout_top.addWidget(self.timeline_pane)
self.timeline_pane.show()
else:
self._timeline = None
self.timeline_pane = None
self._inspectors = {}
# keep a copy of the current message for opening new inspectors
self._current_msg = None
self.tree_all_devices.itemDoubleClicked.connect(self._tree_clicked)
self.warn_flattree.itemDoubleClicked.connect(self._tree_clicked)
self.err_flattree.itemDoubleClicked.connect(self._tree_clicked)
# TODO: resize on itemCollapsed and itemExpanded
self._is_stale = False
self._timer = QTimer()
self._timer.timeout.connect(self._update_message_state)
self._timer.start(1000)
palette = self.tree_all_devices.palette()
self._original_base_color = palette.base().color()
self._original_alt_base_color = palette.alternateBase().color()
self._tree = StatusItem(self.tree_all_devices.invisibleRootItem())
self._warn_tree = StatusItem(self.warn_flattree.invisibleRootItem())
self._err_tree = StatusItem(self.err_flattree.invisibleRootItem())
@Slot(DiagnosticArray)
def message_cb(self, msg):
""" DiagnosticArray message callback """
self._current_msg = msg
# Walk the status array and update the tree
for status in msg.status:
# Compute path and walk to appropriate subtree
path = status.name.split('/')
if path[0] == '':
path = path[1:]
tmp_tree = self._tree
for p in path:
tmp_tree = tmp_tree[p]
tmp_tree.update(status, util.get_resource_name(status.name))
# Check for warnings
if status.level == DiagnosticStatus.WARN:
name = status.name
self._warn_tree[name].update(status, status.name)
# Check for errors
if status.level == DiagnosticStatus.ERROR:
name = status.name
self._err_tree[name].update(status, status.name)
# For any items in the tree that were not updated, remove them
self._tree.prune()
self._warn_tree.prune()
self._err_tree.prune()
# TODO(ahendrix): implement
# Insight: for any item that is not OK, it only provides additional
# information if all of it's children are OK
#
# otherwise, it's just an aggregation of its children
# and doesn't provide any additional value when added to
# the warning and error flat trees
self.tree_all_devices.resizeColumnToContents(0)
self.warn_flattree.resizeColumnToContents(0)
self.err_flattree.resizeColumnToContents(0)
def resizeEvent(self, evt):
"""Overridden from QWidget"""
rospy.logdebug('RobotMonitorWidget resizeEvent')
if self.timeline_pane:
self.timeline_pane.redraw()
@Slot(str)
def _inspector_closed(self, name):
""" Called when an inspector window is closed """
if name in self._inspectors:
del self._inspectors[name]
def _tree_clicked(self, item, column):
"""
Slot to QTreeWidget.itemDoubleClicked
:type item: QTreeWidgetItem
:type column: int
"""
rospy.logdebug('RobotMonitorWidget _tree_clicked col=%d', column)
if item.name in self._inspectors:
self._inspectors[item.name].activateWindow()
else:
self._inspectors[item.name] = InspectorWindow(
self, item.name, self._current_msg, self._timeline)
self._inspectors[item.name].closed.connect(self._inspector_closed)
self.message_updated.connect(
self._inspectors[item.name].message_updated)
def _update_message_state(self):
""" Update the display if it's stale """
if self._timeline is not None:
if self._timeline.has_messages:
previous_stale_state = self._is_stale
self._is_stale = self._timeline.is_stale
time_diff = int(self._timeline.data_age())
msg_template = "Last message received %s %s ago"
if time_diff == 1:
msg = msg_template % (time_diff, "second")
else:
msg = msg_template % (time_diff, "seconds")
self.timeline_pane._msg_label.setText(msg)
if previous_stale_state != self._is_stale:
self._update_background_color()
else:
# no messages received yet
self.timeline_pane._msg_label.setText("No messages received")
def _update_background_color(self):
""" Update the background color based on staleness """
p = self.tree_all_devices.palette()
if self._is_stale:
p.setColor(QPalette.Base, Qt.darkGray)
p.setColor(QPalette.AlternateBase, Qt.lightGray)
else:
p.setColor(QPalette.Base, self._original_base_color)
p.setColor(QPalette.AlternateBase, self._original_alt_base_color)
self.tree_all_devices.setPalette(p)
self.warn_flattree.setPalette(p)
self.err_flattree.setPalette(p)
def shutdown(self):
"""
This needs to be called whenever this class terminates.
This closes all the instances on all trees.
Also unregisters ROS' subscriber, stops timer.
"""
rospy.logdebug('RobotMonitorWidget in shutdown')
names = self._inspectors.keys()
for name in names:
self._inspectors[name].close()
if self._timeline:
self._timeline.shutdown()
self._timer.stop()
del self._timer
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('splitter', self.splitter.saveState())
# TODO(ahendrix): persist the device paths, positions and sizes of any
# inspector windows
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.contains('splitter'):
self.splitter.restoreState(instance_settings.value('splitter'))
else:
self.splitter.setSizes([100, 100, 200])
class BagWidget(QWidget):
"""
Widget for use with Bag class to display and replay bag files
Handles all widget callbacks and contains the instance of BagTimeline for storing visualizing bag data
"""
set_status_text = Signal(str)
def __init__(self, context, publish_clock):
"""
:param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext''
"""
super(BagWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_bag'), 'resource',
'bag_widget.ui')
loadUi(ui_file, self, {'BagGraphicsView': BagGraphicsView})
self.setObjectName('BagWidget')
self._timeline = BagTimeline(context, publish_clock)
self.graphics_view.setScene(self._timeline)
self.graphics_view.resizeEvent = self._resizeEvent
self.graphics_view.setMouseTracking(True)
self.play_icon = QIcon.fromTheme('media-playback-start')
self.pause_icon = QIcon.fromTheme('media-playback-pause')
self.play_button.setIcon(self.play_icon)
self.begin_button.setIcon(QIcon.fromTheme('media-skip-backward'))
self.end_button.setIcon(QIcon.fromTheme('media-skip-forward'))
self.slower_button.setIcon(QIcon.fromTheme('media-seek-backward'))
self.faster_button.setIcon(QIcon.fromTheme('media-seek-forward'))
self.previous_button.setIcon(QIcon.fromTheme('go-previous'))
self.next_button.setIcon(QIcon.fromTheme('go-next'))
self.zoom_in_button.setIcon(QIcon.fromTheme('zoom-in'))
self.zoom_out_button.setIcon(QIcon.fromTheme('zoom-out'))
self.zoom_all_button.setIcon(QIcon.fromTheme('zoom-original'))
self.thumbs_button.setIcon(QIcon.fromTheme('insert-image'))
self.record_button.setIcon(QIcon.fromTheme('media-record'))
self.load_button.setIcon(QIcon.fromTheme('document-open'))
self.save_button.setIcon(QIcon.fromTheme('document-save'))
self.play_button.clicked[bool].connect(self._handle_play_clicked)
self.thumbs_button.clicked[bool].connect(self._handle_thumbs_clicked)
self.zoom_in_button.clicked[bool].connect(self._handle_zoom_in_clicked)
self.zoom_out_button.clicked[bool].connect(
self._handle_zoom_out_clicked)
self.zoom_all_button.clicked[bool].connect(
self._handle_zoom_all_clicked)
self.previous_button.clicked[bool].connect(
self._handle_previous_clicked)
self.next_button.clicked[bool].connect(self._handle_next_clicked)
self.faster_button.clicked[bool].connect(self._handle_faster_clicked)
self.slower_button.clicked[bool].connect(self._handle_slower_clicked)
self.begin_button.clicked[bool].connect(self._handle_begin_clicked)
self.end_button.clicked[bool].connect(self._handle_end_clicked)
self.record_button.clicked[bool].connect(self._handle_record_clicked)
self.load_button.clicked[bool].connect(self._handle_load_clicked)
self.save_button.clicked[bool].connect(self._handle_save_clicked)
self.graphics_view.mousePressEvent = self._timeline.on_mouse_down
self.graphics_view.mouseReleaseEvent = self._timeline.on_mouse_up
self.graphics_view.mouseMoveEvent = self._timeline.on_mouse_move
self.graphics_view.wheelEvent = self._timeline.on_mousewheel
self.closeEvent = self.handle_close
self.keyPressEvent = self.on_key_press
# TODO when the closeEvent is properly called by ROS_GUI implement that event instead of destroyed
self.destroyed.connect(self.handle_destroy)
self.graphics_view.keyPressEvent = self.graphics_view_on_key_press
self.play_button.setEnabled(False)
self.thumbs_button.setEnabled(False)
self.zoom_in_button.setEnabled(False)
self.zoom_out_button.setEnabled(False)
self.zoom_all_button.setEnabled(False)
self.previous_button.setEnabled(False)
self.next_button.setEnabled(False)
self.faster_button.setEnabled(False)
self.slower_button.setEnabled(False)
self.begin_button.setEnabled(False)
self.end_button.setEnabled(False)
self.save_button.setEnabled(False)
self._recording = False
self._timeline.status_bar_changed_signal.connect(
self._update_status_bar)
self.set_status_text.connect(self._set_status_text)
def graphics_view_on_key_press(self, event):
key = event.key()
if key in (Qt.Key_Left, Qt.Key_Right, Qt.Key_Up, Qt.Key_Down,
Qt.Key_PageUp, Qt.Key_PageDown):
# This causes the graphics view to ignore these keys so they can be caught by the bag_widget keyPressEvent
event.ignore()
else:
# Maintains functionality for all other keys QGraphicsView implements
QGraphicsView.keyPressEvent(self.graphics_view, event)
# callbacks for ui events
def on_key_press(self, event):
key = event.key()
if key == Qt.Key_Space:
self._timeline.toggle_play()
elif key == Qt.Key_Home:
self._timeline.navigate_start()
elif key == Qt.Key_End:
self._handle_end_clicked()
elif key == Qt.Key_Plus or key == Qt.Key_Equal:
self._handle_faster_clicked()
elif key == Qt.Key_Minus:
self._handle_slower_clicked()
elif key == Qt.Key_Left:
self._timeline.translate_timeline_left()
elif key == Qt.Key_Right:
self._timeline.translate_timeline_right()
elif key == Qt.Key_Up or key == Qt.Key_PageUp:
self._handle_zoom_in_clicked()
elif key == Qt.Key_Down or key == Qt.Key_PageDown:
self._handle_zoom_out_clicked()
def handle_destroy(self, args):
self._timeline.handle_close()
def handle_close(self, event):
self.shutdown_all()
event.accept()
def _resizeEvent(self, event):
# TODO The -2 allows a buffer zone to make sure the scroll bars do not appear when not needed. On some systems (Lucid) this doesn't function properly
# need to look at a method to determine the maximum size of the scene that will maintain a proper no scrollbar fit in the view.
self.graphics_view.scene().setSceneRect(
0, 0,
self.graphics_view.width() - 2,
max(self.graphics_view.height() - 2,
self._timeline._timeline_frame._history_bottom))
def _handle_publish_clicked(self, checked):
self._timeline.set_publishing_state(checked)
def _handle_play_clicked(self, checked):
if checked:
self.play_button.setIcon(self.pause_icon)
self._timeline.navigate_play()
else:
self.play_button.setIcon(self.play_icon)
self._timeline.navigate_stop()
def _handle_next_clicked(self):
self._timeline.navigate_next()
self.play_button.setChecked(False)
self.play_button.setIcon(self.play_icon)
def _handle_previous_clicked(self):
self._timeline.navigate_previous()
self.play_button.setChecked(False)
self.play_button.setIcon(self.play_icon)
def _handle_faster_clicked(self):
self._timeline.navigate_fastforward()
self.play_button.setChecked(True)
self.play_button.setIcon(self.pause_icon)
def _handle_slower_clicked(self):
self._timeline.navigate_rewind()
self.play_button.setChecked(True)
self.play_button.setIcon(self.pause_icon)
def _handle_begin_clicked(self):
self._timeline.navigate_start()
def _handle_end_clicked(self):
self._timeline.navigate_end()
def _handle_thumbs_clicked(self, checked):
self._timeline._timeline_frame.toggle_renderers()
def _handle_zoom_all_clicked(self):
self._timeline.reset_zoom()
def _handle_zoom_out_clicked(self):
self._timeline.zoom_out()
def _handle_zoom_in_clicked(self):
self._timeline.zoom_in()
def _handle_record_clicked(self):
if self._recording:
self._timeline.toggle_recording()
return
#TODO Implement limiting by regex and by number of messages per topic
self.topic_selection = TopicSelection()
self.topic_selection.recordSettingsSelected.connect(
self._on_record_settings_selected)
def _on_record_settings_selected(self, all_topics, selected_topics):
# TODO verify master is still running
filename = QFileDialog.getSaveFileName(
self, self.tr('Select prefix for new Bag File'), '.',
self.tr('Bag files {.bag} (*.bag)'))
if filename[0] != '':
prefix = filename[0].strip()
# Get filename to record to
record_filename = time.strftime('%Y-%m-%d-%H-%M-%S.bag',
time.localtime(time.time()))
if prefix.endswith('.bag'):
prefix = prefix[:-len('.bag')]
if prefix:
record_filename = '%s_%s' % (prefix, record_filename)
rospy.loginfo('Recording to %s.' % record_filename)
self.load_button.setEnabled(False)
self._recording = True
self._timeline.record_bag(record_filename, all_topics,
selected_topics)
def _handle_load_clicked(self):
filename = QFileDialog.getOpenFileName(
self, self.tr('Load from File'), '.',
self.tr('Bag files {.bag} (*.bag)'))
if filename[0] != '':
self.load_bag(filename[0])
def load_bag(self, filename):
qWarning("Loading %s" % filename)
# QProgressBar can EITHER: show text or show a bouncing loading bar,
# but apparently the text is hidden when the bounding loading bar is
# shown
#self.progress_bar.setRange(0, 0)
self.set_status_text.emit("Loading %s" % filename)
#progress_format = self.progress_bar.format()
#progress_text_visible = self.progress_bar.isTextVisible()
#self.progress_bar.setFormat("Loading %s" % filename)
#self.progress_bar.setTextVisible(True)
bag = rosbag.Bag(filename)
self.play_button.setEnabled(True)
self.thumbs_button.setEnabled(True)
self.zoom_in_button.setEnabled(True)
self.zoom_out_button.setEnabled(True)
self.zoom_all_button.setEnabled(True)
self.next_button.setEnabled(True)
self.previous_button.setEnabled(True)
self.faster_button.setEnabled(True)
self.slower_button.setEnabled(True)
self.begin_button.setEnabled(True)
self.end_button.setEnabled(True)
self.save_button.setEnabled(True)
self.record_button.setEnabled(False)
self._timeline.add_bag(bag)
qWarning("Done loading %s" % filename)
# put the progress bar back the way it was
self.set_status_text.emit("")
#self.progress_bar.setFormat(progress_format)
#self.progress_bar.setTextVisible(progress_text_visible) # causes a segfault :(
#self.progress_bar.setRange(0, 100)
# self clear loading filename
def _handle_save_clicked(self):
filename = QFileDialog.getSaveFileName(
self, self.tr('Save selected region to file...'), '.',
self.tr('Bag files {.bag} (*.bag)'))
if filename[0] != '':
self._timeline.copy_region_to_bag(filename[0])
def _set_status_text(self, text):
if text:
self.progress_bar.setFormat(text)
self.progress_bar.setTextVisible(True)
else:
self.progress_bar.setTextVisible(False)
def _update_status_bar(self):
if self._timeline._timeline_frame.playhead is None or self._timeline._timeline_frame.start_stamp is None:
return
# TODO Figure out why this function is causing a "RuntimeError: wrapped C/C++ object of %S has been deleted" on close if the playhead is moving
try:
# Background Process Status
self.progress_bar.setValue(self._timeline.background_progress)
# Raw timestamp
self.stamp_label.setText(
'%.3fs' % self._timeline._timeline_frame.playhead.to_sec())
# Human-readable time
self.date_label.setText(
bag_helper.stamp_to_str(
self._timeline._timeline_frame.playhead))
# Elapsed time (in seconds)
self.seconds_label.setText(
'%.3fs' %
(self._timeline._timeline_frame.playhead -
self._timeline._timeline_frame.start_stamp).to_sec())
# File size
self.filesize_label.setText(
bag_helper.filesize_to_str(self._timeline.file_size()))
# Play speed
spd = self._timeline.play_speed
if spd != 0.0:
if spd > 1.0:
spd_str = '>> %.0fx' % spd
elif spd == 1.0:
spd_str = '>'
elif spd > 0.0:
spd_str = '> 1/%.0fx' % (1.0 / spd)
elif spd > -1.0:
spd_str = '< 1/%.0fx' % (1.0 / -spd)
elif spd == 1.0:
spd_str = '<'
else:
spd_str = '<< %.0fx' % -spd
self.playspeed_label.setText(spd_str)
else:
self.playspeed_label.setText('')
except:
return
# Shutdown all members
def shutdown_all(self):
self._timeline.handle_close()
class TimelineView(QGraphicsView):
"""
This class draws a graphical representation of a timeline.
This is ONLY the bar and colored boxes.
When you instantiate this class, do NOT forget to call set_init_data to
set necessary data.
"""
paused = Signal(bool)
position_changed = Signal(int)
redraw = Signal()
def __init__(self, parent=None):
"""Cannot take args other than parent due to loadUi limitation."""
super(TimelineView, self).__init__(parent=parent)
self._timeline_marker = QIcon.fromTheme('system-search')
self._min = 0
self._max = 0
self._xpos_marker = -1
self._timeline_marker_width = 15
self._timeline_marker_height = 15
self._last_marker_at = -1
self.setUpdatesEnabled(True)
self._scene = QGraphicsScene(self)
self.setScene(self._scene)
self._levels = None
self.redraw.connect(self._signal_redraw)
def mouseReleaseEvent(self, event):
"""
:type event: QMouseEvent
"""
xpos = self.pos_from_x(event.x())
self.set_marker_pos(xpos)
def mousePressEvent(self, event):
"""
:type event: QMouseEvent
"""
# Pause the timeline
self.paused.emit(True)
xpos = self.pos_from_x(event.x())
self.set_marker_pos(xpos)
def mouseMoveEvent(self, event):
"""
:type event: QMouseEvent
"""
xpos = self.pos_from_x(event.x())
self.set_marker_pos(xpos)
def pos_from_x(self, x):
"""
Get the index in the timeline from the mouse click position
:param x: Position relative to self widget.
:return: Index
"""
width = self.size().width()
# determine value from mouse click
width_cell = width / float(max(len(self._levels), 1))
position = int(floor(x / width_cell))
if position == len(self._levels) - 1:
return -1
return position
@Slot(int)
def set_marker_pos(self, xpos):
"""
Set marker position from index
:param xpos: Marker index
"""
if self._levels is None:
rospy.logwarn('Called set_marker_pos before set_levels')
return
if xpos == -1:
# stick to the latest when position is -1
self._xpos_marker = -1
# check if we chose latest item
if self._last_marker_at != self._xpos_marker:
# update variable to check for change during next round
self._last_marker_at = self._xpos_marker
# emit change to all timeline_panes
self.position_changed.emit(self._xpos_marker)
self.redraw.emit()
return
self._xpos_marker = self._clamp(xpos, self._min, self._max)
if self._xpos_marker == self._last_marker_at:
# Clicked the same pos as last time.
return
elif self._xpos_marker >= len(self._levels):
# When clicked out-of-region
return
self._last_marker_at = self._xpos_marker
# Set timeline position. This broadcasts the message at that position
# to all of the other viewers
self.position_changed.emit(self._xpos_marker)
self.redraw.emit()
def _clamp(self, val, min, max):
"""
Judge if val is within the range given by min & max.
If not, return either min or max.
:type val: any number format
:type min: any number format
:type max: any number format
:rtype: int
"""
if (val < min):
return min
if (val > max):
return max
return val
@Slot(list)
def set_levels(self, levels):
self._levels = levels
self.redraw.emit()
@Slot()
def _signal_redraw(self):
"""
Gets called either when new msg comes in or when marker is moved by
user.
"""
if self._levels is None:
return
# update the limits
self._min = 0
self._max = len(self._levels) - 1
self._scene.clear()
qsize = self.size()
width_tl = qsize.width()
w = width_tl / float(max(len(self._levels), 1))
is_enabled = self.isEnabled()
for i, level in enumerate(self._levels):
h = self.viewport().height()
# Figure out each cell's color.
qcolor = QColor('grey')
if is_enabled and level is not None:
if level > DiagnosticStatus.ERROR:
# Stale items should be reported as errors unless all stale
level = DiagnosticStatus.ERROR
qcolor = util.level_to_color(level)
# TODO Use this code for adding gradation to the cell color.
# end_color = QColor(0.5 * QColor('red').value(),
# 0.5 * QColor('green').value(),
# 0.5 * QColor('blue').value())
self._scene.addRect(w * i, 0, w, h, QColor('white'), qcolor)
# Getting marker index.
xpos_marker = self._xpos_marker
# If marker is -1 for latest use (number_of_cells -1)
if xpos_marker == -1:
xpos_marker = len(self._levels) - 1
# Convert get horizontal pixel value of selected cell's center
xpos_marker_in_pixel = (xpos_marker * w + (w / 2.0) -
(self._timeline_marker_width / 2.0))
pos_marker = QPointF(xpos_marker_in_pixel, 0)
# Need to instantiate marker everytime since it gets deleted
# in every loop by scene.clear()
timeline_marker = self._instantiate_tl_icon()
timeline_marker.setPos(pos_marker)
self._scene.addItem(timeline_marker)
def _instantiate_tl_icon(self):
timeline_marker_icon = QIcon.fromTheme('system-search')
timeline_marker_icon_pixmap = timeline_marker_icon.pixmap(
self._timeline_marker_width, self._timeline_marker_height)
return QGraphicsPixmapItem(timeline_marker_icon_pixmap)
class ScreenWidget(QWidget):
'''
Shows the output of a screen.
'''
clear_signal = Signal()
cleared_signal = Signal()
output = Signal(str)
output_prefix = Signal(str)
error_signal = Signal(str)
auth_signal = Signal(str, str, str) # host, nodename, user
def __init__(self,
masteruri,
screen_name,
nodename,
user=None,
parent=None):
'''
Creates the window, connects the signals and init the class.
'''
QWidget.__init__(self, parent)
# load the UI file
screen_dock_file = os.path.join(
os.path.dirname(os.path.realpath(__file__)), '..', 'ui',
'logscreen', 'ScreenWidget.ui')
loadUi(screen_dock_file, self)
self.setObjectName("ScreenWidget")
self.setWindowIcon(nm.settings().icon('crystal_clear_show_io.png'))
# self.setFeatures(QDockWidget.DockWidgetFloatable | QDockWidget.DockWidgetMovable | QDockWidget.DockWidgetClosable)
self.pauseButton.setIcon(nm.settings().icon('sekkyumu_pause.png'))
self._valid = True
self._lock = threading.RLock()
self.finished = False
self.qfile = None
self.thread = None
self._info = ''
self._masteruri = ''
self._nodename = ''
self._first_fill = True
self._seek_start = -1
self._seek_end = -1
self._pause_read_end = False
self._ssh_output_file = None
self._ssh_error_file = None
self._ssh_input_file = None
self._on_pause = False
self._char_format_end = None
self.loggers.setVisible(False)
self.loglevelButton.toggled.connect(self.on_toggle_loggers)
self.logger_handler = None
# connect to the button signals
self.output.connect(self._on_output)
self.output_prefix.connect(self._on_output_prefix)
self.error_signal.connect(self._on_error)
self.auth_signal.connect(self.on_request_pw)
self.clearCloseButton.clicked.connect(self.clear)
# self.pauseButton.clicked.connect(self.stop)
self.pauseButton.toggled.connect(self.pause)
self.clear_signal.connect(self.clear)
self.textBrowser.verticalScrollBar().valueChanged.connect(
self.on_scrollbar_position_changed)
self.textBrowser.verticalScrollBar().rangeChanged.connect(
self.on_scrollbar_range_changed)
self.textBrowser.set_reader(self)
self.tf = TerminalFormats()
self.hl = ScreenHighlighter(self.textBrowser.document())
self.searchFrame.setVisible(False)
self.grepFrame.setVisible(False)
self.grepLineEdit.textChanged.connect(self.on_grep_changed)
self._shortcut_search = QShortcut(
QKeySequence(self.tr("Ctrl+F", "Activate search")), self)
self._shortcut_search.activated.connect(self.on_search)
self._shortcut_grep = QShortcut(
QKeySequence(self.tr("Ctrl+G", "Activate grep")), self)
self._shortcut_grep.activated.connect(self.on_grep)
self.searchLineEdit.editingFinished.connect(self.on_search_prev)
self.searchNextButton.clicked.connect(self.on_search_next)
self.searchPrevButton.clicked.connect(self.on_search_prev)
# self.visibilityChanged.connect(self.stop)
self._connect(masteruri, screen_name, nodename, user)
def masteruri(self):
return self._masteruri
def name(self):
return self._nodename
def clear(self):
'''
Removes all messages and emit the `cleared_signal`.
'''
self.textBrowser.clear()
self.infoLabel.setText('')
self.cleared_signal.emit()
def finish(self):
self.finished = True
self.output.disconnect()
self.output_prefix.disconnect()
self.close()
def closeEvent(self, event):
self.stop()
QWidget.closeEvent(self, event)
def hide(self):
self.stop()
QWidget.hide(self)
def close(self):
self.stop()
QWidget.close(self)
def on_search(self):
self.searchFrame.setVisible(not self.searchFrame.isVisible())
if self.searchFrame.isVisible():
self.searchLineEdit.setFocus()
self.searchLineEdit.selectAll()
else:
cursor = self.textBrowser.textCursor()
cursor.clearSelection()
self.textBrowser.setTextCursor(cursor)
self.textBrowser.setFocus()
def on_search_next(self):
self._perform_search(forward=True)
def on_search_prev(self):
self._perform_search(forward=False)
def _perform_search(self, forward=False):
search_str = self.searchLineEdit.text()
if search_str:
cursor = self.textBrowser.textCursor()
if forward:
search_result = self.textBrowser.document().find(
search_str, cursor)
else:
search_result = self.textBrowser.document().find(
search_str, cursor, QTextDocument.FindBackward)
if search_result.position() > -1:
self.textBrowser.setTextCursor(search_result)
self.searchLabel.setText('')
# self.searchLabel.setText('%d' % search_result.position())
else:
self.searchLabel.setText('no results')
else:
self.searchLabel.setText('')
def on_grep(self):
self.grepFrame.setVisible(not self.grepFrame.isVisible())
if self.grepFrame.isVisible():
self.grepLineEdit.setFocus()
self.on_grep_changed(self.grepLineEdit.text())
self.hl.set_grep_text('')
self.grepLineEdit.selectAll()
else:
self.on_grep_changed('')
self.textBrowser.setFocus()
def on_grep_changed(self, text):
self.hl.set_grep_text(text)
def stop(self):
'''
'''
if self.qfile is not None and self.qfile.isOpen():
self.qfile.close()
self.qfile = None
self._seek_start = -1
self._seek_end = -1
self._pause_read_end = False
# self.clear()
try:
self._ssh_output_file.close()
self._ssh_error_file.close()
# send Ctrl+C to remote process
self._ssh_input_file.write('%s\n' % chr(3))
self._ssh_input_file.close()
except Exception:
pass
def pause(self, state):
self._on_pause = state
def valid(self):
return self._valid
def _connect(self, masteruri, screen_name, nodename, user=None):
self._masteruri = masteruri
if self.qfile is not None and self.qfile.isOpen():
self.qfile.close()
self.clear_signal.emit()
host = get_hostname(masteruri)
if nm.is_local(host):
self._nodename = nodename
if screen_name:
screen_log = screen.get_logfile(node=nodename)
else:
screen_log = screen.get_ros_logfile(node=nodename)
self.qfile = QFile(screen_log)
self.setWindowTitle(nodename)
if self.qfile.open(QIODevice.ReadOnly):
self._first_fill = True
self.qfile.seek(self.qfile.size() - 1)
# self.lread()
self._info = "END"
self.thread = threading.Thread(target=self._read_log,
kwargs={"filename": screen_log})
self.thread.setDaemon(True)
self.thread.start()
else:
self._valid = False
else:
self._connect_ssh(host, nodename, user)
self.logger_handler = LoggerHandler(
nodename,
masteruri=masteruri,
layout=self.scrollAreaWidgetContents.layout())
self.logger_handler.update()
return False
def _read_log(self, filename, lines=80):
while self.qfile is not None and self.qfile.isOpen():
with self._lock:
if self._first_fill:
chars_count = self._seek_count_lines(lines)
self._seek_start = self.qfile.pos()
data = self.qfile.read(chars_count)
if sys.version_info > (3, 0):
data = data.decode('utf-8')
self.output.emit(data)
self._seek_end = self.qfile.pos()
self._first_fill = False
else:
if self._seek_end != -1:
self.qfile.seek(self._seek_end)
if (not self._pause_read_end
and self.qfile.bytesAvailable()):
start = self.qfile.pos()
data = self.qfile.readAll().data()
if sys.version_info > (3, 0):
data = data.decode('utf-8')
self.output.emit(data)
self._seek_end = self.qfile.pos()
self._info = "NEW: %d" % (self._seek_end - start)
time.sleep(0.25)
def reverse_read(self, lines=20):
with self._lock:
if self.qfile is not None and self.qfile.isOpen():
if lines == -1:
self.qfile.seek(0)
chars_count = self._seek_start
else:
self.qfile.seek(self._seek_start)
chars_count = self._seek_count_lines(lines)
self._seek_start = self.qfile.pos()
data = self.qfile.read(chars_count)
if sys.version_info > (3, 0):
data = data.decode('utf-8')
self.output_prefix.emit(data)
def _seek_count_lines(self, lines=20):
if self.qfile.pos() < 2:
self.qfile.seek(0)
return self.qfile.pos()
count = 0
chars_count = 2
line_size = 0
count_reached = False
self.qfile.seek(self.qfile.pos() - 2)
while (not count_reached) and (self.qfile.pos() > 0):
ch = self.qfile.read(1)
self.qfile.seek(self.qfile.pos() - 2)
chars_count += 1
line_size += 1
if line_size > 120:
count += 1
line_size = 0
if ch == '\n':
count += 1
line_size = 0
if count >= lines:
count_reached = True
return chars_count + 1
def _on_output_prefix(self, msg):
'''
This text will be prepended
'''
if self.finished or self._on_pause:
return
if msg:
cursor = QTextCursor(self.textBrowser.document())
self.tf.insert_formated(cursor, msg.rstrip())
self.textBrowser.setTextCursor(cursor)
self.textBrowser.moveCursor(QTextCursor.Start)
self._update_info_label()
def _on_output(self, msg):
'''
This text will be appended.
'''
if self.finished or self._on_pause:
return
if msg:
at_end = self.textBrowser.verticalScrollBar().value(
) > self.textBrowser.verticalScrollBar().maximum() - 20
cursor_select = self.textBrowser.textCursor()
# store selection and do not scroll to the appended text
if not cursor_select.hasSelection():
cursor_select = None
cursor = self.textBrowser.textCursor()
cursor.movePosition(QTextCursor.End)
if self.hl.has_grep_text():
# grep new text
lines = msg.splitlines(True)
for line in lines:
if self.hl.contains_grep_text(line):
self._char_format_end = self.tf.insert_formated(
cursor, line, char_format=None)
else:
self._char_format_end = self.tf.insert_formated(
cursor, msg, char_format=self._char_format_end)
if cursor_select is not None:
# restore selection
self.textBrowser.setTextCursor(cursor_select)
elif at_end:
self.textBrowser.moveCursor(QTextCursor.End)
self._update_info_label()
if not self.finished:
self.show()
def on_scrollbar_position_changed(self, value):
self._update_info_label()
def on_scrollbar_range_changed(self, min, max):
self._update_info_label()
def _on_error(self, msg):
self.textBrowser.append(msg)
self._update_info_label('SSH ERROR')
def _update_info_label(self, info=''):
info_text = info
vbar_value = self.textBrowser.verticalScrollBar().value()
if not info_text:
if vbar_value == 0:
if self._seek_start == 0:
info_text = 'START'
else:
info_text += "%d %%" % (self._seek_start * 100 /
self._seek_end)
elif vbar_value == self.textBrowser.verticalScrollBar().maximum():
info_text = 'END'
else:
info_text = "%d / %d" % (
vbar_value / 20,
self.textBrowser.verticalScrollBar().maximum() / 20)
self.infoLabel.setText(info_text + '\t%s / %s' %
(sizeof_fmt(self._seek_end - self._seek_start),
sizeof_fmt(self._seek_end)))
def _connect_ssh(self, host, nodename, user=None, pw=None):
try:
if user is not None:
self.infoLabel.setText('connecting to %s@%s' % (user, host))
else:
self.infoLabel.setText('connecting to %s' % host)
ok = False
self.ssh_input_file, self.ssh_output_file, self.ssh_error_file, ok = nm.ssh(
).ssh_exec(host, [
nm.settings().start_remote_script, '--tail_screen_log',
nodename
],
user,
pw,
auto_pw_request=False,
get_pty=True)
if ok:
thread = threading.Thread(target=self._read_ssh_output,
args=((self.ssh_output_file, )))
thread.setDaemon(True)
thread.start()
thread = threading.Thread(target=self._read_ssh_error,
args=((self.ssh_error_file, )))
thread.setDaemon(True)
thread.start()
elif self.ssh_output_file:
self.ssh_output_file.close()
self.ssh_error_file.close()
except nm.AuthenticationRequest as e:
self.auth_signal.emit(host, nodename, user)
except Exception as e:
self.error_signal.emit('%s\n' % e)
def on_request_pw(self, host, nodename, user):
res, user, pw = nm.ssh()._requestPW(user, host)
if res:
self._connect_ssh(host, nodename, user, pw)
def _read_ssh_output(self, output_file):
while not output_file.closed:
text = output_file.readline()
if text:
self.output.emit(text)
def _read_ssh_error(self, error_file):
try:
while not error_file.closed:
text = error_file.readline()
if text:
self.error_signal.emit(text)
except Exception:
pass
def on_toggle_loggers(self, state):
self.loggers.setVisible(state)
if state:
self.logger_handler.update()
class ClassName(QObject):
_update_task_delegates = Signal()
_update_endeffector_widget = Signal()
def __init__(self, context):
QObject.__init__(self, context)
self.setObjectName('ClassName')
# setup publisher
self._examplePublisher = rospy.Publisher('/policy_gui/exampletopic',
Bool,
queue_size=10)
self.goal_client = actionlib.SimpleActionClient(
'/car1/move_base', MoveBaseAction)
self.goal_client.wait_for_server()
self.velocity_service1_ = rospy.ServiceProxy(
'/car1/car_control/pomdp_velocity', ActionObservation)
self.position_service1_ = rospy.ServiceProxy(
'/car1/car_control/pomdp_position', ActionObservation)
self.listener = tf.TransformListener()
# car 2
self._examplePublisher = rospy.Publisher('/policy_gui/exampletopic',
Bool,
queue_size=10)
self.goal_client = actionlib.SimpleActionClient(
'/car2/move_base', MoveBaseAction)
self.goal_client.wait_for_server()
self.velocity_service2_ = rospy.ServiceProxy(
'/car2/car_control/pomdp_velocity', ActionObservation)
self.position_service2_ = rospy.ServiceProxy(
'/car2/car_control/pomdp_position', ActionObservation)
self.listener = tf.TransformListener()
# setup services
#self.getObjects = rospy.ServiceProxy('worldmodel/get_object_model', GetObjectModel)
# setup subscribers
#self._worldmodelObjectsSubscriber = rospy.Subscriber("/worldmodel/objects", ObjectModel, self._on_worldmodel_objects)
#self._interactive_marker_endeffector = rospy.Subscriber("/interactive_marker_pose_control/feedback",InteractiveMarkerFeedback,
#self._on_interactive_marker_endeffector_pose,queue_size=1)
# setup action clients
#self._move_arm_client = actionlib.SimpleActionClient("/move_group", MoveGroupAction)
#setup tf listener
#self.tf_listener = TransformListener()
# setup main widget
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('policy_gui'), 'lib',
'ClassName.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('ClassNameUi')
#set connections
self._widget.start_button.pressed.connect(
self._on_start_button_pressed)
self._widget.setup_button.pressed.connect(
self._on_setup_button_pressed)
#getRobotJoints_button
# 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)
#taurob_training_week = rospy.get_param('/taurob_training_week',False)
# connect Signal Slot
#self._update_task_delegates.connect(self._on_update_task_delegates)
# init stuff
# self.bla = 1
self.pos_car1 = []
self.pos_car2 = []
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 _on_joint_states(self,message):
# arm_joints =['arm_joint_0', 'arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4']
def compute_action(self, state_before):
#compute action
# STATE OBSERVATION 1:
# print('Observing state before action:')
# print('car position1:', self.pos_car1)
# print('car position2:', self.pos_car2)
#
# distance_before = math.sqrt((self.pos_car1[0] - self.pos_car2[0]) ** 2 + (self.pos_car1[1] - self.pos_car2[1]) ** 2)
# print('distance between cars: ', distance_before)
state_before = np.zeros((10.1))
state_before[0] = self.pos_car1[0] # car1 x possition
state_before[1] = self.pos_car1[1] # car1 y possition
state_before[2] = self.velocity_service1_.call(
req) # car1 velocity NOT SURE ABOUT THIS
state_before[
3] = goal.target_pose.pose.position.x # car1 goal x NOT SURE ABOUT THIS
state_before[
4] = goal.target_pose.pose.position.y # car1 goal y NOT SURE ABOUT THIS
state_before[5] = self.pos_car2[0] # car2 x possition
state_before[6] = self.pos_car2[1] # car2 y possition
state_before[7] = self.velocity_service2_.call(
req) # car1 velocity NOT SURE ABOUT THIS
state_before[
8] = goal.target_pose.pose.position.x # car1 goal x NOT SURE ABOUT THIS
state_before[
9] = goal.target_pose.pose.position.y # car1 goal y NOT SURE ABOUT THIS
# state_before = State()
# state_before.car1_pose.position.x = self.pos_car1[0] # car1 x possition
# state_before.car1_pose.position.y = self.pos_car1[1] # car1 y possition
# state_before.car1_vel = self.velocity_service1_.call(req) # car1 velocity NOT SURE ABOUT THIS
# state_before.car1_goal = goal.target_pose.pose # car1 goal x NOT SURE ABOUT THIS
# state_before.car2_pose.position.x = self.pos_car2[0] # car2 x possition
# state_before.car2_pose.position.y = self.pos_car2[1] # car2 y possition
# state_before.car2_vel = self.velocity_service2_.call(req) # car1 velocity NOT SURE ABOUT THIS
# state_before.car2_goal = goal.target_pose.pose # car1 goal x NOT SURE ABOUT THIS
print('Making an action:')
actions = [3] * 65 + [4] * 10 + [2] * 10 + [1] * 15
random_action = random.choice(actions)
print(random_action)
req = ActionObservationRequest()
req.action = random_action
res = self.velocity_service1_.call(req)
time.sleep(3.0) # --- I guess we don't need, but?
return state_before, random_action
def transition(self, state_before, action):
#compute action
# return observation + next state
# ------------------------------------------------------------------------------
# 'state_before' here instead of 'self.pos_car1' ?
# ------------------------------------------------------------------------------
state_next = np.zeros((10.1))
state_next[0] = self.pos_car1[0] # car1 x possition
state_next[1] = self.pos_car1[1] # car1 y possition
state_next[2] = self.velocity_service1_.call(
req) # car1 velocity NOT SURE ABOUT THIS
state_next[
3] = goal.target_pose.pose.position.x # car1 goal x NOT SURE ABOUT THIS
state_next[
4] = goal.target_pose.pose.position.y # car1 goal y NOT SURE ABOUT THIS
state_next[5] = self.pos_car2[0] # car2 x possition
state_next[6] = self.pos_car2[1] # car2 y possition
state_next[7] = self.velocity_service2_.call(
req) # car1 velocity NOT SURE ABOUT THIS
state_next[
8] = goal.target_pose.pose.position.x # car1 goal x NOT SURE ABOUT THIS
state_next[
9] = goal.target_pose.pose.position.y # car1 goal y NOT SURE ABOUT THIS
observation = np.zeros((7.1))
observation[0] = self.pos_car1[0] # car1 x possition
observation[1] = self.pos_car1[1] # car1 y possition
observation[2] = self.velocity_service1_.call(
req) # car1 velocity NOT SURE ABOUT THIS
observation[3] = self.pos_car2[0] # car2 x possition
observation[4] = self.pos_car2[1] # car2 y possition
observation[5] = self.velocity_service2_.call(
req) # car2 velocity NOT SURE ABOUT THIS
observation[6] = math.sqrt((self.pos_car1[0] - self.pos_car2[0])**2 +
(self.pos_car1[1] - self.pos_car2[1])**2)
# observation = Observation()
# observation.car1_pose.position.x = self.pos_car1[0]
# observation.car1_pose.position.y = self.pos_car1[1]
# observation.car1_vel = self.velocity_service1_.call(req)
# observation.car2_pose.position.x = self.pos_car2[0]
# observation.car2_pose.position.y = self.pos_car2[1]
# observation.car2_vel = self.velocity_service2_.call(req)
# observation.distance_between_cars = math.sqrt((self.pos_car1[0] - self.pos_car2[0]) ** 2 + (self.pos_car1[1] - self.pos_car2[1]) ** 2)
return state_next, observation
def reward(self, state_next, action, observation=None):
# Negative reward for the action that was made ( for all actions the same negative reward)
# Negative reward, if the car is to close to the other car (pre-defined range)
# Speed reward ???
# Positive reward, if the car is close to its goal (pre-defined range)
print('Computing reward:')
if observation.distance_between_cars >= 10:
reward = 5
elif 10 > observation.distance_between_cars >= 5:
reward = 0
elif 5 > observation.distance_between_cars >= 2:
reward = -10
elif 2 > observation.distance_between_cars >= 0:
reward = -100
#print('Step reward: ', reward)
#print('Total reward: ', reward_total)
return reward
# compute action
# return observation + next state
def _on_setup_button_pressed(self):
# should send two navigation goals
print(' Setup Button pressed, publishing msg')
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "/car1/map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 45.0
goal.target_pose.pose.position.y = 17.0
goal.target_pose.pose.orientation.z = 150.841592654
self.goal_client.send_goal(goal)
# moves only first stops
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "/car2/map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 33.0
goal.target_pose.pose.position.y = 45.0
goal.target_pose.pose.orientation.z = 20.441592654
self.goal_client.send_goal(goal)
time_now = rospy.Time(0)
(trans1, rot1) = self.listener.lookupTransform('/car1/base_link',
'/map', time_now)
(trans2, rot2) = self.listener.lookupTransform('/car2/base_link',
'/map', time_now)
self.pos_car1 = trans1
self.pos_car2 = trans2
print("car 1: ")
print(self.pos_car1)
print("car 2: ")
print(self.pos_car2)
def _on_start_button_pressed(self):
# should call compute policy method and compute policy will give list of actions.
# Should execute one action after another (kinda loop). Before or together send
# velocity to another car
self.compute_policy()
print(' Start Button pressed, publishing msg')
def compute_policy(self, state_before):
# random number generator with 3 for 50% of the times, 1 - 30%, 4 and 2 - both 10%
# 1 - dec.; 2 - hold; 3 - acc.; 4 - stop
goal_x = 45.0
goal_y = 17.0
reward_total = 0
while self.pos_car1[0] < goal_x and self.pos_car1[
1] < goal_y: # TODO: think about that + real time possition of cars + moving car2
state_before, action = self.compute_action(self, state_before)
state_next, observation = self.transition(self, state_before,
action)
reward_total += self.reward(self,
state_next,
action,
observation=None)
print 'Total reward:', reward_total
print('Randomization is over')
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 QwtDataPlot(Qwt.QwtPlot):
mouseCoordinatesChanged = Signal(QPointF)
_colors = [
Qt.red, Qt.blue, Qt.magenta, Qt.cyan, Qt.green, Qt.darkYellow,
Qt.black, Qt.darkRed, Qt.gray, Qt.darkCyan
]
_num_value_saved = 1000
_num_values_ploted = 1000
def __init__(self, *args):
super(QwtDataPlot, self).__init__(*args)
self.setCanvasBackground(Qt.white)
self.insertLegend(Qwt.QwtLegend(), Qwt.QwtPlot.BottomLegend)
self._curves = {}
self._data_offset_x = 0
self._canvas_offset_x = 0
self._canvas_offset_y = 0
self._last_canvas_x = 0
self._last_canvas_y = 0
self._pressed_canvas_y = 0
self._last_click_coordinates = None
self._color_index = 0
marker_axis_y = Qwt.QwtPlotMarker()
marker_axis_y.setLabelAlignment(Qt.AlignRight | Qt.AlignTop)
marker_axis_y.setLineStyle(Qwt.QwtPlotMarker.HLine)
marker_axis_y.setYValue(0.0)
marker_axis_y.attach(self)
self._picker = Qwt.QwtPlotPicker(Qwt.QwtPlot.xBottom,
Qwt.QwtPlot.yLeft,
Qwt.QwtPicker.PolygonSelection,
Qwt.QwtPlotPicker.PolygonRubberBand,
Qwt.QwtPicker.AlwaysOn, self.canvas())
self._picker.setRubberBandPen(QPen(self._colors[-1]))
self._picker.setTrackerPen(QPen(self._colors[-1]))
# Initialize data
self._time_axis = arange(self._num_values_ploted)
self._canvas_display_height = 1000
self._canvas_display_width = self.canvas().width()
self._data_offset_x = self._num_value_saved - len(self._time_axis)
self.redraw()
self.move_canvas(0, 0)
self.canvas().setMouseTracking(True)
self.canvas().installEventFilter(self)
def eventFilter(self, _, event):
if event.type() == QEvent.MouseButtonRelease:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
self._last_click_coordinates = QPointF(x, y)
elif event.type() == QEvent.MouseMove:
x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x())
y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y())
coords = QPointF(x, y)
if self._picker.isActive(
) and self._last_click_coordinates is not None:
toolTip = 'origin x: %.5f, y: %.5f' % (
self._last_click_coordinates.x(),
self._last_click_coordinates.y())
delta = coords - self._last_click_coordinates
toolTip += '\ndelta x: %.5f, y: %.5f\nlength: %.5f' % (
delta.x(), delta.y(), QVector2D(delta).length())
else:
toolTip = 'buttons\nleft: measure\nmiddle: move\nright: zoom x/y\nwheel: zoom y'
self.setToolTip(toolTip)
self.mouseCoordinatesChanged.emit(coords)
return False
def log(self, level, message):
self.emit(SIGNAL('logMessage'), level, message)
def resizeEvent(self, event):
#super(QwtDataPlot, self).resizeEvent(event)
Qwt.QwtPlot.resizeEvent(self, event)
self.rescale()
def add_curve(self, curve_id, curve_name, values_x, values_y):
curve_id = str(curve_id)
if self._curves.get(curve_id):
return
curve_object = Qwt.QwtPlotCurve(curve_name)
curve_object.attach(self)
curve_object.setPen(
QPen(self._colors[self._color_index % len(self._colors)]))
self._color_index += 1
self._curves[curve_id] = {
'name': curve_name,
'data': zeros(self._num_value_saved),
'object': curve_object,
}
def remove_curve(self, curve_id):
curve_id = str(curve_id)
if curve_id in self._curves:
self._curves[curve_id]['object'].hide()
self._curves[curve_id]['object'].attach(None)
del self._curves[curve_id]['object']
del self._curves[curve_id]
@Slot(str, list, list)
def update_values(self, curve_id, values_x, values_y):
for value_x, value_y in zip(values_x, values_y):
self.update_value(curve_id, value_x, value_y)
@Slot(str, float, float)
def update_value(self, curve_id, value_x, value_y):
curve_id = str(curve_id)
# update data plot
if curve_id in self._curves:
# TODO: use value_x as timestamp
self._curves[curve_id]['data'] = concatenate(
(self._curves[curve_id]['data'][1:],
self._curves[curve_id]['data'][:1]), 1)
self._curves[curve_id]['data'][-1] = float(value_y)
def redraw(self):
for curve_id in self._curves.keys():
self._curves[curve_id]['object'].setData(
self._time_axis, self._curves[curve_id]['data']
[self._data_offset_x:self._data_offset_x +
len(self._time_axis)])
#self._curves[curve_id]['object'].setStyle(Qwt.QwtPlotCurve.CurveStyle(3))
self.replot()
def rescale(self):
y_num_ticks = self.height() / 40
y_lower_limit = self._canvas_offset_y - (self._canvas_display_height /
2)
y_upper_limit = self._canvas_offset_y + (self._canvas_display_height /
2)
# calculate a fitting step size for nice, round tick labels, depending on the displayed value area
y_delta = y_upper_limit - y_lower_limit
exponent = int(math.log10(y_delta))
presicion = -(exponent - 2)
y_step_size = round(y_delta / y_num_ticks, presicion)
self.setAxisScale(Qwt.QwtPlot.yLeft, y_lower_limit, y_upper_limit,
y_step_size)
self.setAxisScale(Qwt.QwtPlot.xBottom, 0, len(self._time_axis))
self.redraw()
def rescale_axis_x(self, delta__x):
new_len = len(self._time_axis) + delta__x
new_len = max(10, min(new_len, self._num_value_saved))
self._time_axis = arange(new_len)
self._data_offset_x = max(
0,
min(self._data_offset_x,
self._num_value_saved - len(self._time_axis)))
self.rescale()
def scale_axis_y(self, max_value):
self._canvas_display_height = max_value
self.rescale()
def move_canvas(self, delta_x, delta_y):
self._data_offset_x += delta_x * len(self._time_axis) / float(
self.canvas().width())
self._data_offset_x = max(
0,
min(self._data_offset_x,
self._num_value_saved - len(self._time_axis)))
self._canvas_offset_x += delta_x * self._canvas_display_width / self.canvas(
).width()
self._canvas_offset_y += delta_y * self._canvas_display_height / self.canvas(
).height()
self.rescale()
def mousePressEvent(self, event):
self._last_canvas_x = event.x() - self.canvas().x()
self._last_canvas_y = event.y() - self.canvas().y()
self._pressed_canvas_y = event.y() - self.canvas().y()
def mouseMoveEvent(self, event):
canvas_x = event.x() - self.canvas().x()
canvas_y = event.y() - self.canvas().y()
if event.buttons() & Qt.MiddleButton: # middle button moves the canvas
delta_x = self._last_canvas_x - canvas_x
delta_y = canvas_y - self._last_canvas_y
self.move_canvas(delta_x, delta_y)
elif event.buttons() & Qt.RightButton: # right button zooms
zoom_factor = max(
-0.6, min(0.6, (self._last_canvas_y - canvas_y) / 20.0 / 2.0))
delta_y = (self.canvas().height() / 2.0) - self._pressed_canvas_y
self.move_canvas(0, zoom_factor * delta_y * 1.0225)
self.scale_axis_y(
max(
0.005, self._canvas_display_height -
(zoom_factor * self._canvas_display_height)))
self.rescale_axis_x(self._last_canvas_x - canvas_x)
self._last_canvas_x = canvas_x
self._last_canvas_y = canvas_y
def wheelEvent(self, event): # mouse wheel zooms the y-axis
canvas_y = event.y() - self.canvas().y()
zoom_factor = max(-0.6, min(0.6, (event.delta() / 120) / 6.0))
delta_y = (self.canvas().height() / 2.0) - canvas_y
self.move_canvas(0, zoom_factor * delta_y * 1.0225)
self.scale_axis_y(
max(
0.0005, self._canvas_display_height -
zoom_factor * self._canvas_display_height))
class TextSearchFrame(QDockWidget):
'''
A frame to find text in the Editor.
'''
search_result_signal = Signal(str, bool, str, int)
''' @ivar: A signal emitted after search_threaded was started.
(search text, found or not, file, position in text)
for each result a signal will be emitted.
'''
replace_signal = Signal(str, str, int, str)
''' @ivar: A signal emitted to replace string at given position.
(search text, file, position in text, replaced by text)
'''
def __init__(self, tabwidget, parent=None):
QDockWidget.__init__(self, "Find", parent)
self.setObjectName('SearchFrame')
self.setFeatures(QDockWidget.DockWidgetMovable
| QDockWidget.DockWidgetFloatable)
self._dockwidget = QFrame(self)
self.vbox_layout = QVBoxLayout(self._dockwidget)
self.layout().setContentsMargins(0, 0, 0, 0)
self.layout().setSpacing(1)
# frame with two rows for find and replace
find_replace_frame = QFrame(self)
find_replace_vbox_layout = QVBoxLayout(find_replace_frame)
find_replace_vbox_layout.setContentsMargins(0, 0, 0, 0)
find_replace_vbox_layout.setSpacing(1)
# find_replace_vbox_layout.addSpacerItem(QSpacerItem(1, 1, QSizePolicy.Expanding, QSizePolicy.Expanding))
# create frame with find row
find_frame = self._create_find_frame()
find_replace_vbox_layout.addWidget(find_frame)
rplc_frame = self._create_replace_frame()
find_replace_vbox_layout.addWidget(rplc_frame)
# frame for find&replace and search results
self.vbox_layout.addWidget(find_replace_frame)
self.vbox_layout.addWidget(self._create_found_frame())
# self.vbox_layout.addStretch(2024)
self.setWidget(self._dockwidget)
# intern search parameters
self._tabwidget = tabwidget
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self._search_result_index = -1
self._search_recursive = False
self._search_thread = None
def _create_find_frame(self):
find_frame = QFrame(self)
find_hbox_layout = QHBoxLayout(find_frame)
find_hbox_layout.setContentsMargins(0, 0, 0, 0)
find_hbox_layout.setSpacing(1)
self.search_field = EnchancedLineEdit(find_frame)
self.search_field.setPlaceholderText('search text')
self.search_field.textChanged.connect(self.on_search_text_changed)
self.search_field.returnPressed.connect(self.on_search)
find_hbox_layout.addWidget(self.search_field)
self.search_result_label = QLabel(find_frame)
self.search_result_label.setText(' ')
find_hbox_layout.addWidget(self.search_result_label)
self.find_button_back = QPushButton("<")
self.find_button_back.setFixedWidth(44)
self.find_button_back.clicked.connect(self.on_search_back)
find_hbox_layout.addWidget(self.find_button_back)
self.find_button = QPushButton(">")
self.find_button.setDefault(True)
# self.find_button.setFlat(True)
self.find_button.setFixedWidth(44)
self.find_button.clicked.connect(self.on_search)
find_hbox_layout.addWidget(self.find_button)
return find_frame
def _create_replace_frame(self):
# create frame with replace row
self.rplc_frame = rplc_frame = QFrame(self)
rplc_hbox_layout = QHBoxLayout(rplc_frame)
rplc_hbox_layout.setContentsMargins(0, 0, 0, 0)
rplc_hbox_layout.setSpacing(1)
self.replace_field = EnchancedLineEdit(rplc_frame)
self.replace_field.setPlaceholderText('replace text')
self.replace_field.returnPressed.connect(self.on_replace)
rplc_hbox_layout.addWidget(self.replace_field)
self.replace_result_label = QLabel(rplc_frame)
self.replace_result_label.setText(' ')
rplc_hbox_layout.addWidget(self.replace_result_label)
self.replace_button = replace_button = QPushButton("> &Replace >")
replace_button.setFixedWidth(90)
replace_button.clicked.connect(self.on_replace_click)
rplc_hbox_layout.addWidget(replace_button)
rplc_frame.setVisible(False)
return rplc_frame
def _create_found_frame(self):
ff_frame = QFrame(self)
self.found_files_vbox_layout = QVBoxLayout(ff_frame)
self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0)
self.recursive_search_box = QCheckBox("recursive search")
self.found_files_vbox_layout.addWidget(self.recursive_search_box)
self.found_files_list = QTreeWidget(ff_frame)
self.found_files_list.setColumnCount(1)
self.found_files_list.setFrameStyle(QFrame.StyledPanel)
self.found_files_list.setHeaderHidden(True)
self.found_files_list.itemActivated.connect(self.on_itemActivated)
self.found_files_list.setStyleSheet("QTreeWidget {"
"background-color:transparent;"
"}"
"QTreeWidget::item {"
"background-color:transparent;"
"}"
"QTreeWidget::item:selected {"
"background-color: darkgray;"
"}")
self.found_files_vbox_layout.addWidget(self.found_files_list)
self.recursive_search_box.setChecked(False)
return ff_frame
def keyPressEvent(self, event):
'''
Enable the shortcats for search and replace
'''
self.parent().keyPressEvent(event)
def on_search(self):
'''
Initiate the new search or request a next search result.
'''
if self.current_search_text != self.search_field.text(
) or self._search_recursive != self.recursive_search_box.isChecked():
# clear current search results
self._reset()
self.current_search_text = self.search_field.text()
if self.current_search_text:
path_text = {}
self._wait_for_result = True
for i in range(self._tabwidget.count()):
path_text[self._tabwidget.widget(
i).filename] = self._tabwidget.widget(
i).document().toPlainText()
self._search_recursive = self.recursive_search_box.isChecked()
self._search_thread = TextSearchThread(
self.current_search_text,
self._tabwidget.currentWidget().filename,
path_text=path_text,
recursive=self._search_recursive)
self._search_thread.search_result_signal.connect(
self.on_search_result)
self._search_thread.warning_signal.connect(
self.on_warning_result)
self._search_thread.start()
elif self.search_results:
self._check_position()
if self.search_results:
if self._search_result_index + 1 >= len(self.search_results):
self._search_result_index = -1
self._search_result_index += 1
(id, search_text, found, path, index, linenr,
line) = self.search_results[self._search_result_index]
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
self._update_label()
def on_search_back(self):
'''
Slot to handle the search back function.
'''
self._check_position(False)
if self.search_results:
self._search_result_index -= 1
if self._search_result_index < 0:
self._search_result_index = len(self.search_results) - 1
self._update_label()
(id, search_text, found, path, index, linenr,
line) = self.search_results[self._search_result_index]
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
def _check_position(self, forward=True):
try:
# if the position of the textCursor was changed by the user, move the search index
cur_pos = self._tabwidget.currentWidget().textCursor().position()
id, st, _f, pa, idx, lnr, ltxt = self.search_results[
self._search_result_index]
sear_pos = idx + len(st)
if cur_pos != sear_pos:
first_idx = self._get_current_index_for_current_file()
if first_idx != -1:
id, st, _f, pa, idx, lnr, ltxt = self.search_results[
first_idx]
sear_pos = idx + len(st)
while cur_pos > sear_pos and self._tabwidget.currentWidget(
).filename == pa:
first_idx += 1
id, st, _f, pa, idx, lnr, ltxt = self.search_results[
first_idx]
sear_pos = idx + len(st)
self._search_result_index = first_idx
if forward:
self._search_result_index -= 1
else:
self._reset(True)
except:
pass
def _get_current_index_for_current_file(self):
for index in range(len(self.search_results)):
id, _st, _f, pa, _idx = self.search_results[index]
if self._tabwidget.currentWidget().filename == pa:
return index
return -1
def on_search_result(self, search_text, found, path, index, linenr, line):
'''
Slot to handle the signals for search result. This signals are forwarded used
search_result_signal.
'''
if found and search_text == self.current_search_text:
id = "%d:%s" % (index, path)
self.search_results_fileset.add(path)
item = (search_text, found, path, index)
if item not in self.search_results:
self.search_results.append(
(id, search_text, found, path, index, linenr, line))
if self._wait_for_result:
self._search_result_index += 1
if index >= self._tabwidget.currentWidget().textCursor(
).position() or self._tabwidget.currentWidget(
).filename != path:
self._wait_for_result = False
self.search_result_signal.emit(search_text, found, path,
index)
self.replace_button.setEnabled(True)
pkg, rpath = package_name(os.path.dirname(path))
itemstr = '%s [%s]' % (os.path.basename(path), pkg)
if not self.found_files_list.findItems(itemstr, Qt.MatchExactly):
list_item = QTreeWidgetItem(self.found_files_list)
list_item.setText(0, itemstr)
list_item.setToolTip(0, path)
self.found_files_list.insertTopLevelItem(0, list_item)
self.found_files_list.expandAll()
for i in range(self.found_files_list.topLevelItemCount()):
top_item = self.found_files_list.topLevelItem(i)
if top_item.text(0) == itemstr:
sub_item_str = "%d: %s" % (linenr, line)
list_item2 = QTreeWidgetItem()
list_item2.setText(0, sub_item_str)
list_item2.setWhatsThis(0, id)
top_item.addChild(list_item2)
#self.found_files_list.setVisible(len(self.search_results_fileset) > 0)
self._update_label()
def on_warning_result(self, text):
rospy.logwarn(text)
def on_replace_click(self):
self.on_replace()
self.on_search()
def on_replace(self):
'''
Emits the replace signal, but only if currently selected text is equal to the searched one.
'''
if self.search_results:
try:
id, search_text, _found, path, index, linenr, line_text = self.search_results[
self._search_result_index]
cursor = self._tabwidget.currentWidget().textCursor()
if cursor.selectedText() == search_text:
rptxt = self.replace_field.text()
for rindex in range(self._search_result_index + 1,
len(self.search_results)):
iid, st, _f, pa, idx, lnr, ltxt = self.search_results[
rindex]
if path == pa:
self.search_results.pop(rindex)
self.search_results.insert(
rindex,
(iid, st, _f, pa, idx + len(rptxt) - len(st),
lnr, ltxt))
else:
break
self._remove_search_result(self._search_result_index)
self._search_result_index -= 1
self.replace_signal.emit(search_text, path, index, rptxt)
else:
self.replace_button.setEnabled(False)
except:
import traceback
print traceback.format_exc()
pass
def on_itemActivated(self, item):
'''
Go to the results for the selected file entry in the list.
'''
splits = item.whatsThis(0).split(':')
if len(splits) == 2:
item_index = int(splits[0])
item_path = splits[1]
new_search_index = -1
tmp_index = -1
search_index = -1
tmp_search_text = ''
for id, search_text, found, path, index, linenr, line_text in self.search_results:
new_search_index += 1
if item_path == path and item_index == index:
self._search_result_index = new_search_index
self.search_result_signal.emit(search_text, found, path,
index)
self._update_label()
def on_search_text_changed(self, _text):
'''
Clear search result if the text was changed.
'''
self._reset()
def _update_label(self, clear_label=False):
'''
Updates the status label for search results. The info is created from search result lists.
'''
msg = ' '
if self.search_results:
count_files = len(self.search_results_fileset)
msg = '%d/%d' % (self._search_result_index + 1,
len(self.search_results))
if count_files > 1:
msg = '%s(%d)' % (msg, count_files)
if self._search_thread is not None and self._search_thread.is_alive():
msg = 'searching..%s' % msg
elif not msg.strip() and self.current_search_text:
msg = '0 found'
self.current_search_text = ''
if clear_label:
msg = ' '
self.search_result_label.setText(msg)
self.find_button_back.setEnabled(len(self.search_results))
self._select_current_item_in_box(self._search_result_index)
def file_changed(self, path):
'''
Clears search results if for changed file are some search results are available
:param path: changed file path
:type path: str
'''
if path in self.search_results_fileset:
self._reset()
def set_replace_visible(self, value):
self.rplc_frame.setVisible(value)
self.raise_()
self.activateWindow()
if value:
self.replace_field.setFocus()
self.replace_field.selectAll()
self.setWindowTitle("Find / Replace")
else:
self.setWindowTitle("Find")
self.search_field.setFocus()
def is_replace_visible(self):
return self.rplc_frame.isVisible()
def _reset(self, force_new_search=False):
# clear current search results
if self._search_thread is not None:
self._search_thread.search_result_signal.disconnect()
self._search_thread.stop()
self._search_thread = None
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self.found_files_list.clear()
# self.found_files_list.setVisible(False)
self._update_label(True)
self._search_result_index = -1
self.find_button_back.setEnabled(False)
if force_new_search:
self.on_search()
def enable(self):
self.setVisible(True)
# self.show()
self.raise_()
self.activateWindow()
self.search_field.setFocus()
self.search_field.selectAll()
def _select_current_item_in_box(self, index):
try:
(id, search_text, found, path, index, linenr,
line) = self.search_results[index]
for topidx in range(self.found_files_list.topLevelItemCount()):
topitem = self.found_files_list.topLevelItem(topidx)
for childdx in range(topitem.childCount()):
child = topitem.child(childdx)
if child.whatsThis(0) == id:
child.setSelected(True)
elif child.isSelected():
child.setSelected(False)
except:
pass
def _remove_search_result(self, index):
try:
(id, search_text, found, path, index, linenr,
line) = self.search_results.pop(index)
pkg, rpath = package_name(os.path.dirname(path))
itemstr = '%s [%s]' % (os.path.basename(path), pkg)
found_items = self.found_files_list.findItems(
itemstr, Qt.MatchExactly)
for item in found_items:
for chi in range(item.childCount()):
child = item.child(chi)
if child.whatsThis(0) == id:
item.removeChild(child)
break
# delete top level item if it is now empty
for topidx in range(self.found_files_list.topLevelItemCount()):
if self.found_files_list.topLevelItem(
topidx).childCount() == 0:
self.found_files_list.takeTopLevelItem(topidx)
break
# create new set with files contain the search text
new_path_set = set(path for _id, _st, _fd, path, _idx, lnr, lntxt
in self.search_results)
self.search_results_fileset = new_path_set
# self.found_files_list.setVisible(len(self.search_results_fileset) > 0)
except:
import traceback
print traceback.format_exc()
class PyQtGraphDataPlot(QWidget):
limits_changed = Signal()
def __init__(self, parent=None):
super(PyQtGraphDataPlot, self).__init__(parent)
self._plot_widget = PlotWidget()
self._plot_widget.getPlotItem().addLegend()
self._plot_widget.setBackground((255, 255, 255))
self._plot_widget.setXRange(0, 10, padding=0)
vbox = QVBoxLayout()
vbox.addWidget(self._plot_widget)
self.setLayout(vbox)
self._plot_widget.getPlotItem().sigRangeChanged.connect(
self.limits_changed)
self._curves = {}
self._current_vline = None
def add_curve(self,
curve_id,
curve_name,
curve_color=QColor(Qt.blue),
markers_on=False):
pen = mkPen(curve_color, width=1)
symbol = "o"
symbolPen = mkPen(QColor(Qt.black))
symbolBrush = mkBrush(curve_color)
# this adds the item to the plot and legend
if markers_on:
plot = self._plot_widget.plot(name=curve_name,
pen=pen,
symbol=symbol,
symbolPen=symbolPen,
symbolBrush=symbolBrush,
symbolSize=4)
else:
plot = self._plot_widget.plot(name=curve_name, pen=pen)
self._curves[curve_id] = plot
def remove_curve(self, curve_id):
curve_id = str(curve_id)
if curve_id in self._curves:
self._plot_widget.removeItem(self._curves[curve_id])
del self._curves[curve_id]
self._update_legend()
def _update_legend(self):
# clear and rebuild legend (there is no remove item method for the legend...)
self._plot_widget.clear()
self._plot_widget.getPlotItem().legend.items = []
for curve in self._curves.values():
self._plot_widget.addItem(curve)
if self._current_vline:
self._plot_widget.addItem(self._current_vline)
def redraw(self):
pass
def set_values(self, curve_id, data_x, data_y):
curve = self._curves[curve_id]
curve.setData(data_x, data_y)
def vline(self, x, color):
if self._current_vline:
self._plot_widget.removeItem(self._current_vline)
self._current_vline = self._plot_widget.addLine(x=x, pen=color)
def set_xlim(self, limits):
# TODO: this doesn't seem to handle fast updates well
self._plot_widget.setXRange(limits[0], limits[1], padding=0)
def set_ylim(self, limits):
self._plot_widget.setYRange(limits[0], limits[1], padding=0)
def get_xlim(self):
x_range, _ = self._plot_widget.viewRange()
return x_range
def get_ylim(self):
_, y_range = self._plot_widget.viewRange()
return y_range
class Smach(Plugin):
update_graph_sig = Signal(str)
def __init__(self, context):
super(Smach, self).__init__(context)
self.initialized = 0
self._dotcode_sub = None
self._topic_dict = {}
self._update_thread = WorkerThread(self._update_thread_run,
self._update_finished)
# Give QObjects reasonable names
self.setObjectName('Smach')
# 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_smach.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('SmachPluginUi')
# 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)
#Connect widgets with corresponding methods
self._widget.namespace_input.currentIndexChanged.connect(
self._handle_ns_changed)
self._widget.ns_refresh_button.clicked.connect(self.refresh_combo_box)
self._widget.restrict_ns.clicked.connect(self.refresh_combo_box)
self._widget.ud_path_input.currentIndexChanged.connect(
self._handle_ud_path)
self._widget.ud_set_initial.clicked.connect(self._handle_ud_set_path)
self._widget.ud_text_browser.setReadOnly(1)
self._widget.show_implicit_button.clicked.connect(
self._handle_show_implicit)
self._widget.help_button.setIcon(QIcon.fromTheme('help-contents'))
self._widget.help_button.clicked.connect(self._handle_help)
self._widget.tree.clicked.connect(self._handle_tree_clicked)
#Depth and width spinners:
self._widget.depth_input.setRange(-1, 1337)
self._widget.depth_input.setValue(-1)
self._widget.depth_input.valueChanged.connect(self._set_depth)
self._widget.label_width_input.setRange(1, 1337)
self._widget.label_width_input.setValue(40)
self._widget.label_width_input.valueChanged.connect(self._set_width)
self._widget.tree.setColumnCount(1)
self._widget.tree.setHeaderLabels(["Containers"])
self._widget.tree.show()
self._ns = ""
self.refresh_combo_box()
# Bind path list
self._widget.path_input.currentIndexChanged.connect(
self._handle_path_changed)
#Keep Combo Boxes sorted
self._widget.namespace_input.setInsertPolicy(6)
self._widget.path_input.setInsertPolicy(6)
self._widget.ud_path_input.setInsertPolicy(6)
#Set up mouse actions for xdot widget
self._widget.xdot_widget.register_select_callback(self.select_cb)
# Create graph data structures
# Containers is a map of (container path) -> container proxy
self._containers = {}
self._top_containers = {}
# smach introspection client
self._client = smach_ros.IntrospectionClient()
self._selected_paths = []
# Message subscribers
self._structure_subs = {}
self._status_subs = {}
# Initialize xdot display state
self._path = '/'
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._label_wrapper = textwrap.TextWrapper(40, break_long_words=True)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._keep_running = True
self._update_server_list()
self._update_graph()
self._update_tree()
# Start a timer to update the server list
self._server_timer = QTimer(self)
self._server_timer.timeout.connect(self._update_server_list)
self._server_timer.start(1000)
# Start a timer to update the graph display
self._graph_timer = QTimer(self)
self._graph_timer.timeout.connect(self._update_graph)
self._graph_timer.start(1093)
# Start a timer to update the._widget.tree display
self._tree_timer = QTimer(self)
self._tree_timer.timeout.connect(self._update_tree)
self._tree_timer.start(1217)
def _handle_tree_clicked(self):
selected = self._widget.tree.selectedItems()[0]
path = "/" + str(selected.text(0))
parent = selected.parent()
while parent:
path = "/" + str(parent.text(0)) + path
parent = parent.parent()
self._widget.ud_path_input.setCurrentIndex(
self._widget.ud_path_input.findText(path))
def _handle_help(self):
"""Event: Help button pressed"""
helpMsg = QMessageBox()
helpMsg.setWindowTitle("Keyboard Controls")
helpMsg.setIcon(QMessageBox.Information)
helpMsg.setText(
"Pan: Arrow Keys\nZoom: PageUp / PageDown\nZoom To Fit: F\nRefresh: R"
)
ret = helpMsg.exec_()
def select_cb(self, event):
"""Event: Click to select a graph node to display user data and update the graph."""
# Only set string status
x = event.x()
y = event.y()
url = self._widget.xdot_widget.get_url(x, y)
if url is None:
return
item = self._widget.xdot_widget.items_by_url.get(url.url, None)
if item:
self._widget.status_bar.showMessage(item.url)
# Left button-up
if item.url not in self._containers:
if ':' in item.url:
container_url = '/'.join(item.url.split(':')[:-1])
else:
container_url = '/'.join(item.url.split('/')[:-1])
else:
container_url = item.url
if event.button() == Qt.LeftButton:
self._selected_paths = [item.url]
self._widget.ud_path_input.setCurrentIndex(
self._widget.ud_path_input.findText(item.url))
self._graph_needs_refresh = True
def _handle_show_implicit(self):
"""Event: Show Implicit button checked"""
self._show_all_transitions = not self._show_all_transitions
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _set_depth(self):
"""Event: Depth value changed"""
self._max_depth = self._widget.depth_input.value()
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _set_width(self):
"""Event: Label Width value changed"""
self._label_wrapper.width = self._widget.label_width_input.value()
self._graph_needs_refresh = True
def _handle_ud_set_path(self):
"""Event: Set as Initial State button pressed"""
state_path = self._widget.ud_path_input.currentText()
parent_path = get_parent_path(state_path)
if len(parent_path) > 0:
state = get_label(state_path)
server_name = self._containers[parent_path]._server_name
self._client.set_initial_state(server_name,
parent_path, [state],
timeout=rospy.Duration(60.0))
self._structure_changed = True
self._graph_needs_refresh = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def _handle_ud_path(self):
"""Event: User Data selection combo box changed"""
path = self._widget.ud_path_input.currentText()
#Check the path is non-zero length
if len(path) > 0:
# Get the container corresponding to this path, since userdata is
# stored in the containers
if path not in self._containers:
parent_path = get_parent_path(path)
else:
parent_path = path
if parent_path not in self._containers:
parent_path = get_parent_path(parent_path)
if parent_path in self._containers:
# Get the container
container = self._containers[parent_path]
# Generate the userdata string
ud_str = ''
for (k, v) in container._local_data._data.iteritems():
ud_str += str(k) + ": "
vstr = str(v)
# Add a line break if this is a multiline value
if vstr.find('\n') != -1:
ud_str += '\n'
ud_str += vstr + '\n\n'
#Display the user data
self._widget.ud_text_browser.setPlainText(ud_str)
self._widget.ud_text_browser.show()
def _update_server_list(self):
"""Update the list of known SMACH introspection servers."""
# Discover new servers
if self._widget.restrict_ns.isChecked():
server_names = [self._widget.namespace_input.currentText()[0:-1]]
#self._status_subs = {}
else:
server_names = self._client.get_servers()
new_server_names = [
sn for sn in server_names if sn not in self._status_subs
]
# Create subscribers for newly discovered servers
for server_name in new_server_names:
# Create a subscriber for the plan structure (topology) published by this server
self._structure_subs[server_name] = rospy.Subscriber(
server_name + smach_ros.introspection.STRUCTURE_TOPIC,
SmachContainerStructure,
callback=self._structure_msg_update,
callback_args=server_name,
queue_size=50)
# Create a subscriber for the active states in the plan published by this server
self._status_subs[server_name] = rospy.Subscriber(
server_name + smach_ros.introspection.STATUS_TOPIC,
SmachContainerStatus,
callback=self._status_msg_update,
queue_size=50)
def _structure_msg_update(self, msg, server_name):
"""Update the structure of the SMACH plan (re-generate the dotcode)."""
# Just return if we're shutting down
if not self._keep_running:
return
# Get the node path
path = msg.path
pathsplit = path.split('/')
parent_path = '/'.join(pathsplit[0:-1])
rospy.logdebug("RECEIVED: " + path)
rospy.logdebug("CONTAINERS: " + str(self._containers.keys()))
# Initialize redraw flag
needs_redraw = False
# Determine if we need to update one of the proxies or create a new one
if path in self._containers:
rospy.logdebug("UPDATING: " + path)
# Update the structure of this known container
needs_redraw = self._containers[path].update_structure(msg)
else:
rospy.logdebug("CONSTRUCTING: " + path)
# Create a new container
container = ContainerNode(server_name, msg)
self._containers[path] = container
#Add items to ud path combo box
if self._widget.ud_path_input.findText(path) == -1:
self._widget.ud_path_input.addItem(path)
for item in container._children:
if self._widget.ud_path_input.findText(path + "/" +
item) == -1:
self._widget.ud_path_input.addItem(path + "/" + item)
# Store this as a top container if it has no parent
if parent_path == '':
self._top_containers[path] = container
# Append the path to the appropriate widgets
self._append_new_path(path)
# We need to redraw the graph if this container's parent is being viewed
if path.find(self._widget.path_input.currentText()) == 0:
needs_redraw = True
if needs_redraw:
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
self._tree_needs_refresh = True
self._graph_needs_refresh = True
def _status_msg_update(self, msg):
"""Process status messages."""
# Check if we're in the process of shutting down
if not self._keep_running:
return
# Get the path to the updating conainer
path = msg.path
rospy.logdebug("STATUS MSG: " + path)
# Check if this is a known container
if path in self._containers:
# Get the container and check if the status update requires regeneration
container = self._containers[path]
if container.update_status(msg):
self._graph_needs_refresh = True
self._tree_needs_refresh = True
def _append_new_path(self, path):
"""Append a new path to the path selection widgets"""
if ((not self._widget.restrict_ns.isChecked())
or ((self._widget.restrict_ns.isChecked()) and
(self._widget.namespace_input.currentText() in path))
or (path == self._widget.namespace_input.currentText()[0:-1])):
self._widget.path_input.addItem(path)
def _update_graph(self):
"""This thread continuously updates the graph when it changes.
The graph gets updated in one of two ways:
1: The structure of the SMACH plans has changed, or the display
settings have been changed. In this case, the dotcode needs to be
regenerated.
2: The status of the SMACH plans has changed. In this case, we only
need to change the styles of the graph.
"""
if self._keep_running and self._graph_needs_refresh and not rospy.is_shutdown(
):
# Get the containers to update
containers_to_update = {}
# Check if the path that's currently being viewed is in the
# list of existing containers
if self._path in self._containers:
# Some non-root path
containers_to_update = {
self._path: self._containers[self._path]
}
elif self._path == '/':
# Root path
containers_to_update = self._top_containers
# Check if we need to re-generate the dotcode (if the structure changed)
# TODO: needs_zoom is a misnomer
if self._structure_changed or self._needs_zoom:
dotstr = "digraph {\n\t"
dotstr += ';'.join([
"compound=true",
"outputmode=nodesfirst",
"labeljust=l",
"nodesep=0.5",
"minlen=2",
"mclimit=5",
"clusterrank=local",
"ranksep=0.75",
# "remincross=true",
# "rank=sink",
"ordering=\"\"",
])
dotstr += ";\n"
# Generate the rest of the graph
# TODO: Only re-generate dotcode for containers that have changed
for path, container in containers_to_update.items():
dotstr += container.get_dotcode(self._selected_paths, [],
0, self._max_depth,
self._containers,
self._show_all_transitions,
self._label_wrapper)
# The given path isn't available
if len(containers_to_update) == 0:
dotstr += '"__empty__" [label="Path not available.", shape="plaintext"]'
dotstr += '\n}\n'
# Set the dotcode to the new dotcode, reset the flags
self.set_dotcode(dotstr, zoom=False)
self._structure_changed = False
self._graph_needs_refresh = False
# Update the styles for the graph if there are any updates
for path, container in containers_to_update.items():
container.set_styles(self._selected_paths, 0, self._max_depth,
self._widget.xdot_widget.items_by_url,
self._widget.xdot_widget.subgraph_shapes,
self._containers)
self._widget.xdot_widget.repaint()
def set_dotcode(self, dotcode, zoom=True):
"""Set the xdot view's dotcode and refresh the display."""
# Set the new dotcode
if self._widget.xdot_widget.set_dotcode(dotcode, False):
# Re-zoom if necessary
if zoom or self._needs_zoom:
self._widget.xdot_widget.zoom_to_fit()
self._needs_zoom = False
def _update_tree(self):
"""Update the tree view."""
if self._keep_running and self._tree_needs_refresh and not rospy.is_shutdown(
):
self._tree_nodes = {}
self._widget.tree.clear()
for path, tc in self._top_containers.iteritems():
if ((not self._widget.restrict_ns.isChecked()) or
((self._widget.restrict_ns.isChecked()) and
(self._widget.namespace_input.currentText() in path)) or
(path
== self._widget.namespace_input.currentText()[0:-1])):
self.add_to_tree(path, None)
self._tree_needs_refresh = False
self._widget.tree.sortItems(0, 0)
def add_to_tree(self, path, parent):
"""Add a path to the tree view."""
if parent is None:
container = QTreeWidgetItem()
container.setText(0, self._containers[path]._label)
self._widget.tree.addTopLevelItem(container)
else:
container = QTreeWidgetItem(parent)
container.setText(0, self._containers[path]._label)
# Add children to_tree
for label in self._containers[path]._children:
child_path = '/'.join([path, label])
if child_path in self._containers.keys():
self.add_to_tree(child_path, container)
else:
child = QTreeWidgetItem(container)
child.setText(0, label)
def append_tree(self, container, parent=None):
"""Append an item to the tree view."""
if not parent:
node = QTreeWidgetItem()
node.setText(0, container._label)
self._widget.tree.addTopLevelItem(node)
for child_label in container._children:
child = QTreeWidgetItem(node)
child.setText(0, child_label)
def _update_thread_run(self):
"""Update the list of namespaces."""
_, _, topic_types = rospy.get_master().getTopicTypes()
self._topic_dict = dict(topic_types)
keys = list(self._topic_dict.keys())
namespaces = list()
for i in keys:
print i
if i.endswith("smach/container_status"):
namespaces.append(i[0:i.index("smach/container_status")])
self._widget.namespace_input.setItems.emit(namespaces)
@Slot()
def _update_finished(self):
"""Enable namespace combo box."""
self._widget.namespace_input.setEnabled(True)
def _handle_ns_changed(self):
"""If namespace selection is changed then reinitialize everything."""
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)
rospy.loginfo("Creating action clients on namespace '%s'..." %
ns)
rospy.loginfo("Action clients created.")
self._actions_connected = True
self.enable_widgets(True)
self._containers = {}
self._top_containers = {}
self._selected_paths = []
self._structure_subs = {}
self._status_subs = {}
self._needs_zoom = True
self._structure_changed = True
self._max_depth = -1
self._show_all_transitions = False
self._graph_needs_refresh = True
self._tree_needs_refresh = True
#self._widget.namespace_input.clear()
self._widget.path_input.clear()
self._widget.ud_path_input.clear()
self._widget.tree.clear()
@Slot()
def refresh_combo_box(self):
"""Refresh namespace combo box."""
self._update_thread.kill()
self._containers = {}
self._top_containers = {}
self._selected_paths = []
self._structure_subs = {}
self._status_subs = {}
self._structure_changed = True
self._tree_needs_refresh = True
#self._graph_needs_refresh = True
#self._widget.namespace_input.clear()
self._widget.path_input.clear()
self._widget.ud_path_input.clear()
self._widget.tree.clear()
if self._widget.restrict_ns.isChecked():
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('updating...')
self._widget.ns_refresh_button.setEnabled(True)
self._update_thread.start()
else:
self._widget.namespace_input.setEnabled(False)
self._widget.namespace_input.setEditText('Unrestricted')
self._widget.ns_refresh_button.setEnabled(False)
self._graph_needs_refresh = True
self._tree_needs_refresh = True
self._widget.path_input.addItem('/')
def _handle_path_changed(self, checked):
"""If the path input is changed, update graph."""
self._path = self._widget.path_input.currentText()
self._graph_needs_refresh = True
self._structure_changed = True
if self._widget.zoom_checkbox.isChecked():
self._needs_zoom = True
def enable_widgets(self, enable):
"""Enable all widgets."""
self._widget.xdot_widget.setEnabled(enable)
self._widget.path_input.setEnabled(enable)
self._widget.depth_input.setEnabled(enable)
self._widget.label_width_input.setEnabled(enable)
self._widget.ud_path_input.setEnabled(enable)
self._widget.ud_text_browser.setEnabled(enable)
class PyQtGraphDataPlot(QWidget):
limits_changed = Signal()
def __init__(self, parent=None):
super(PyQtGraphDataPlot, self).__init__(parent)
self._plot_widget = PlotWidget()
self._plot_widget.getPlotItem().addLegend()
self._plot_widget.setBackground((255, 255, 255))
self._plot_widget.setXRange(0, 10, padding=0)
vbox = QVBoxLayout()
vbox.addWidget(self._plot_widget)
self.setLayout(vbox)
self._plot_widget.getPlotItem().sigRangeChanged.connect(
self.limits_changed)
self.bins = 10
self.window = 100
self._curves = {}
self._current_vline = None
def add_curve(self,
curve_id,
curve_name,
curve_color=QColor(Qt.blue),
markers_on=False):
pen = mkPen(curve_color, width=1)
# this adds the item to the plot and legend
plot = self._plot_widget.plot(stepMode=True,
fillLevel=0,
brush=(0, 0, 255, 150))
self._curves[curve_id] = plot
def remove_curve(self, curve_id):
curve_id = str(curve_id)
if curve_id in self._curves:
self._plot_widget.removeItem(self._curves[curve_id])
del self._curves[curve_id]
self._update_legend()
def _update_legend(self):
# clear and rebuild legend (there is no remove item method for the legend...)
self._plot_widget.clear()
self._plot_widget.getPlotItem().legend.items = []
for curve in self._curves.values():
self._plot_widget.addItem(curve)
if self._current_vline:
self._plot_widget.addItem(self._current_vline)
def redraw(self):
pass
def set_values(self, curve_id, data_x, data_y):
curve = self._curves[curve_id]
if len(data_y) > 0:
y, x = numpy.histogram(data_y[-self.window:], self.bins)
curve.setData(x, y)
else:
curve.clear()
self._plot_widget.autoRange()
def vline(self, x, color):
if self._current_vline:
self._plot_widget.removeItem(self._current_vline)
self._current_vline = self._plot_widget.addLine(x=x, pen=color)
def set_xlim(self, limits):
# TODO: this doesn't seem to handle fast updates well
self._plot_widget.setXRange(limits[0], limits[1], padding=0)
def set_ylim(self, limits):
self._plot_widget.setYRange(limits[0], limits[1], padding=0)
def get_xlim(self):
x_range, _ = self._plot_widget.viewRange()
return x_range
def get_ylim(self):
_, y_range = self._plot_widget.viewRange()
return y_range
class MessageFrame(QFrame):
accept_signal = Signal(int, MessageData)
''' @ivar: A signal on accept button clicked (id, data)'''
cancel_signal = Signal(int, MessageData)
''' @ivar: A signal on cancel button clicked (id, data)'''
TYPE_INVALID = 0
TYPE_EMPTY = 1
TYPE_QUESTION = 2
TYPE_LAUNCH_FILE = 3
TYPE_DEFAULT_CFG = 4
TYPE_NODELET = 5
TYPE_TRANSFER = 6
TYPE_BINARY = 7
TYPE_NOSCREEN = 8
TYPE_NMD = 9
TYPE_NODE_CFG = 10
ICON_SIZE = 32
def __init__(self, parent=None, info=False):
QFrame.__init__(self, parent=parent)
self.setObjectName('MessageFrame')
self.questionid = self.TYPE_INVALID
self.text = ""
self.data = MessageData(None)
self.IMAGES = {
1:
QPixmap(),
2:
QPixmap(':/icons/question.png').scaled(self.ICON_SIZE,
self.ICON_SIZE,
Qt.IgnoreAspectRatio,
Qt.SmoothTransformation),
3:
QPixmap(':/icons/crystal_clear_launch_file.png').scaled(
self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation),
4:
QPixmap(":/icons/default_cfg.png").scaled(self.ICON_SIZE,
self.ICON_SIZE,
Qt.IgnoreAspectRatio,
Qt.SmoothTransformation),
5:
QPixmap(":/icons/crystal_clear_nodelet_q.png").scaled(
self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation),
6:
QPixmap(":/icons/crystal_clear_launch_file_transfer.png").scaled(
self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation),
7:
QPixmap(":/icons/crystal_clear_binary.png").scaled(
self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation),
8:
QPixmap(":/icons/crystal_clear_no_io.png").scaled(
self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation),
9:
QPixmap(":/icons/crystal_clear_run_zeroconf.png").scaled(
self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation),
10:
QPixmap(":/icons/sekkyumu_restart.png").scaled(
self.ICON_SIZE, self.ICON_SIZE, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation)
}
self._new_request = False
self.frameui = QFrame()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'MessageFrame.ui')
loadUi(ui_file, self.frameui)
color = QColor(255, 207, 121)
self.frameui.listLabel.setTextInteractionFlags(
Qt.TextSelectableByMouse)
self.frameui.questionLabel.setTextInteractionFlags(
Qt.TextSelectableByMouse)
self.frameui.setVisible(False)
self.frameui.listLabel.setVisible(False)
self.frameui.questionOkButton.clicked.connect(self._on_question_ok)
self.frameui.questionCancelButton.clicked.connect(
self._on_question_cancel)
self.frameui.checkBox_dnaa.stateChanged.connect(
self._on_checkbox_state_changed)
self._ask = 'ask'
if info:
color = QColor(232, 104, 80)
self.frameui.questionCancelButton.setVisible(False)
self._ask = 'show'
bg_style = "QFrame#questionFame { background-color: %s;}" % color.name(
)
self.frameui.setStyleSheet("%s" % (bg_style))
self._queue = MessageQueue()
self._do_not_ask = {}
def show_question(self,
questionid,
text,
data=MessageData(None),
color=None):
if questionid == 0:
return
try:
if questionid == self.TYPE_LAUNCH_FILE and nm.settings(
).autoreload_launch:
self.accept_signal.emit(questionid, data)
return
# is it in the list for not ask again?
if self._do_not_ask[questionid] == 1:
self.accept_signal.emit(questionid, data)
elif self._do_not_ask[questionid] == 0:
self.cancel_signal.emit(questionid, data)
return
except Exception:
pass
if self.questionid != questionid or self.text != text or data != self.data:
self._queue.add(questionid, text, data)
elif data.data_list: # same question again
# update the list of files or nodes which causes this question in current question
for dt in data.data_list:
if dt not in self.data.data_list:
self.data.data_list.append(dt)
self._update_list_label(self.data.data_list)
# if no question is active pop first element from the queue
if self.questionid == self.TYPE_INVALID:
self._new_request = self._read_next_item()
self._frameui_4_request(self._new_request)
if self.questionid in [self.TYPE_NODELET, self.TYPE_NOSCREEN]:
self.frameui.checkBox_dnaa.setText("don't %s again, never!" %
self._ask)
else:
self.frameui.checkBox_dnaa.setText(
"don't %s again, for session" % self._ask)
def show_info(self, infoid, text, data=MessageData(None), color=None):
self.show_question(infoid, text=text, data=data, color=color)
def is_do_not_ask(self, questionid):
try:
# is it in the list for not ask again?
return self._do_not_ask[questionid] in [0, 1]
except Exception:
return False
def hide_question(self, questionids):
for qid in questionids:
self._queue.remove(qid)
if self.questionid in questionids:
self._new_request = False
self.frameui.setVisible(False)
self.cancel_signal.emit(self.questionid, self.data)
self.questionid = 0
self._update_list_label([])
self._new_request = self._read_next_item()
self._frameui_4_request(self._new_request)
def _update_list_label(self, items=[]):
'''
Put list elements into the list label in the question frame
'''
if items:
self.frameui.listLabel.setText('')
for item in items:
ltext = self.frameui.listLabel.text()
if ltext:
self.frameui.listLabel.setText(
"%s, %s" % (ltext, HTMLDelegate.toHTML(item)))
else:
self.frameui.listLabel.setText("%s" %
(HTMLDelegate.toHTML(item)))
self.frameui.listLabel.setVisible(True)
else:
self.frameui.listLabel.setText('')
self.frameui.listLabel.setVisible(False)
def _frameui_4_request(self, request):
if request:
self.frameui.checkBox_dnaa.setChecked(False)
self.frameui.setVisible(True)
self.frameui.listLabel.setVisible(True)
else:
self.questionid = 0
self.frameui.setVisible(False)
self.frameui.listLabel.setVisible(False)
def _on_question_ok(self):
self._new_request = False
self.frameui.setVisible(False)
try:
# set action for do not ask again
if self.frameui.checkBox_dnaa.isChecked():
self._do_not_ask[self.questionid] = 1
except Exception:
pass
self.accept_signal.emit(self.questionid, self.data)
self.questionid = 0
self._update_list_label([])
self._new_request = self._read_next_item()
self._frameui_4_request(self._new_request)
def _on_question_cancel(self):
self._new_request = False
self.frameui.setVisible(False)
try:
# set action for do not ask again
if self.frameui.checkBox_dnaa.isChecked():
self._do_not_ask[self.questionid] = 0
except Exception:
pass
self.cancel_signal.emit(self.questionid, self.data)
self.questionid = 0
self._update_list_label([])
self._new_request = self._read_next_item()
self._frameui_4_request(self._new_request)
def _is_launch_data_in_queue(self, newdata):
for _, data, _ in self._queue_launchfile:
if data == newdata:
return True
return False
def _is_transfer_data_in_queue(self, newdata):
for _, data, _ in self._queue_transfer_files:
if data == newdata:
return True
return False
def _is_other_data_in_queue(self, questionid, text, data):
for cqid, ctxt, cd, _ in self._queue_other:
if cqid == questionid and cd == data and ctxt == text:
return True
return False
def _read_next_item(self):
(qid, text, data) = self._queue.get()
if qid != self.TYPE_INVALID:
self.questionid = qid
self.text = text
self.data = data
self.frameui.questionIcon.setPixmap(self.IMAGES[qid])
self.frameui.questionLabel.setText(text)
self._update_list_label(self.data.data_list)
return qid != self.TYPE_INVALID
def _on_checkbox_state_changed(self, state):
if self.questionid == self.TYPE_NODELET:
self.frameui.questionOkButton.setVisible(not state)
nm.settings().check_for_nodelets_at_start = not state
elif self.questionid == self.TYPE_NOSCREEN:
self.frameui.questionCancelButton.setVisible(not state)
nm.settings().show_noscreen_error = not state
def _clear_scroll_area(self):
child = self.frameui.scrollAreaLayout.takeAt(0)
while child:
child.widget().setParent(None)
del child
child = self.frameui.scrollAreaLayout.takeAt(0)
class PublisherTreeModel(MessageTreeModel):
_column_names = ['topic', 'type', 'rate', 'expression']
item_value_changed = Signal(int, str, str, str, object)
def __init__(self, parent=None):
super(PublisherTreeModel, self).__init__(parent)
self._column_index = {}
for column_name in self._column_names:
self._column_index[column_name] = len(self._column_index)
self.clear()
self._item_change_lock = threading.Lock()
self.itemChanged.connect(self.handle_item_changed)
def clear(self):
super(PublisherTreeModel, self).clear()
self.setHorizontalHeaderLabels(self._column_names)
def get_publisher_ids(self, index_list):
return [
item._user_data['publisher_id']
for item in self._get_toplevel_items(index_list)
]
def remove_items_with_parents(self, index_list):
for item in self._get_toplevel_items(index_list):
self.removeRow(item.row())
def handle_item_changed(self, item):
if not self._item_change_lock.acquire(False):
#qDebug('PublisherTreeModel.handle_item_changed(): could not acquire lock')
return
# lock has been acquired
topic_name = item._path
column_name = self._column_names[item.column()]
if item.isCheckable():
new_value = str(item.checkState() == Qt.Checked)
else:
new_value = item.text().strip()
#print 'PublisherTreeModel.handle_item_changed(): %s, %s, %s' % (topic_name, column_name, new_value)
self.item_value_changed.emit(item._user_data['publisher_id'],
topic_name, column_name, new_value,
item.setText)
# release lock
self._item_change_lock.release()
def remove_publisher(self, publisher_id):
for top_level_row_number in range(self.rowCount()):
item = self.item(top_level_row_number)
if item is not None and item._user_data[
'publisher_id'] == publisher_id:
self.removeRow(top_level_row_number)
return top_level_row_number
return None
def update_publisher(self, publisher_info):
top_level_row_number = self.remove_publisher(
publisher_info['publisher_id'])
self.add_publisher(publisher_info, top_level_row_number)
def add_publisher(self, publisher_info, top_level_row_number=None):
# recursively create widget items for the message's slots
parent = self
slot = publisher_info['message_instance']
slot_name = publisher_info['topic_name']
slot_type_name = publisher_info['message_instance']._type
slot_path = publisher_info['topic_name']
user_data = {'publisher_id': publisher_info['publisher_id']}
kwargs = {
'user_data': user_data,
'top_level_row_number': top_level_row_number,
'expressions': publisher_info['expressions'],
}
top_level_row = self._recursive_create_items(parent, slot, slot_name,
slot_type_name, slot_path,
**kwargs)
# fill tree widget columns of top level item
if publisher_info['enabled']:
top_level_row[self._column_index['topic']].setCheckState(
Qt.Checked)
top_level_row[self._column_index['rate']].setText(
str(publisher_info['rate']))
def _get_data_items_for_path(self, slot_name, slot_type_name, slot_path,
**kwargs):
if slot_name.startswith('/'):
return (CheckableItem(slot_name), ReadonlyItem(slot_type_name),
QStandardItem(''), ReadonlyItem(''))
expression_item = QStandardItem('')
expression_item.setToolTip(
'enter valid Python expression here, using "i" as counter and functions from math, random and time modules'
)
return (ReadonlyItem(slot_name), QStandardItem(slot_type_name),
ReadonlyItem(''), expression_item)
def _recursive_create_items(self,
parent,
slot,
slot_name,
slot_type_name,
slot_path,
expressions={},
**kwargs):
row, is_leaf_node = super(PublisherTreeModel,
self)._recursive_create_items(
parent,
slot,
slot_name,
slot_type_name,
slot_path,
expressions=expressions,
**kwargs)
if is_leaf_node:
expression_text = expressions.get(slot_path, repr(slot))
row[self._column_index['expression']].setText(expression_text)
return row
def flags(self, index):
flags = super(PublisherTreeModel, self).flags(index)
if (index.column() == self._column_index['expression']
and index.model().data(
index.model().index(
index.row(), self._column_index['type'],
index.parent()), Qt.DisplayRole) == 'bool'):
flags |= Qt.ItemIsUserCheckable
return flags
def data(self, index, role):
if (index.column() == self._column_index['expression']
and index.model().data(
index.model().index(
index.row(), self._column_index['type'],
index.parent()), Qt.DisplayRole) == 'bool'):
if role == Qt.CheckStateRole:
value = index.model().data(
index.model().index(index.row(), index.column(),
index.parent()), Qt.DisplayRole)
if value == 'True':
return Qt.Checked
if value == 'False':
return Qt.Unchecked
return Qt.PartiallyChecked
return super(PublisherTreeModel, self).data(index, role)
def setData(self, index, value, role):
if (index.column() == index.column() ==
self._column_index['expression'] and index.model().data(
index.model().index(
index.row(), self._column_index['type'],
index.parent()), Qt.DisplayRole) == 'bool'):
if role == Qt.CheckStateRole:
value = str(value == Qt.Checked)
return QStandardItemModel.setData(self, index, value,
Qt.EditRole)
return QStandardItemModel.setData(self, index, value, role)
class MainWindow(DockableMainWindow):
"""Main window of the application managing the geometry and state of all top-level widgets."""
save_settings_before_close_signal = Signal(Settings, Settings)
def __init__(self):
super(MainWindow, self).__init__()
self.setObjectName('MainWindow')
self._save_on_close_signaled = False
self._global_settings = None
self._perspective_settings = None
self._settings = None
def closeEvent(self, event):
qDebug('MainWindow.closeEvent()')
if not self._save_on_close_signaled:
self._save_geometry_to_perspective()
self._save_state_to_perspective()
self._save_on_close_signaled = True
self.save_settings_before_close_signal.emit(
self._global_settings, self._perspective_settings)
event.ignore()
else:
event.accept()
def save_settings(self, global_settings, perspective_settings):
qDebug('MainWindow.save_settings()')
self._global_settings = global_settings
self._perspective_settings = perspective_settings
self._settings = self._perspective_settings.get_settings('mainwindow')
# store current setup to current _settings
self._save_geometry_to_perspective()
self._save_state_to_perspective()
def restore_settings(self, global_settings, perspective_settings):
qDebug('MainWindow.restore_settings()')
# remember new _settings
self._global_settings = global_settings
self._perspective_settings = perspective_settings
self._settings = self._perspective_settings.get_settings('mainwindow')
# only restore geometry, restoring state is triggered after PluginManager has been updated
self._restore_geometry_from_perspective()
def save_setup(self):
qDebug('MainWindow.save_setup()')
# store current setup to current _settings
self._save_geometry_to_perspective()
self._save_state_to_perspective()
def restore_state(self):
qDebug('MainWindow.restore_state()')
# restore state from _settings
self._restore_state_from_perspective()
def perspective_changed(self, name):
self.setWindowTitle('%s - rqt' % str(name))
def _save_geometry_to_perspective(self):
if self._settings is not None:
# unmaximizing widget before saveGeometry works around bug to restore dock-widgets
# still the non-maximized size can not correctly be restored
maximized = self.isMaximized()
if maximized:
self.showNormal()
self._settings.set_value('geometry', self.saveGeometry())
if maximized:
self.showMaximized()
def _restore_geometry_from_perspective(self):
if self._settings.contains('geometry'):
self.restoreGeometry(self._settings.value('geometry'))
def _save_state_to_perspective(self):
if self._settings is not None:
self._settings.set_value('state', self.saveState())
# safe area for all toolbars
toolbar_settings = self._settings.get_settings('toolbar_areas')
for toolbar in self.findChildren(QToolBar):
area = self.toolBarArea(toolbar)
if area in [Qt.LeftToolBarArea,
Qt.RightToolBarArea,
Qt.TopToolBarArea,
Qt.BottomToolBarArea]:
toolbar_settings.set_value(toolbar.objectName(), area)
def _restore_state_from_perspective(self):
if self._settings.contains('state'):
self.restoreState(self._settings.value('state'))
# restore area for all toolbars
toolbar_settings = self._settings.get_settings('toolbar_areas')
for toolbar in self.findChildren(QToolBar):
if not toolbar.objectName():
continue
area = Qt.ToolBarArea(
int(toolbar_settings.value(toolbar.objectName(), Qt.NoToolBarArea)))
if area in [Qt.LeftToolBarArea,
Qt.RightToolBarArea,
Qt.TopToolBarArea,
Qt.BottomToolBarArea]:
self.addToolBar(area, toolbar)
class SupervisedPopen(QObject):
'''
The class overrides the subprocess.Popen and waits in a thread for its finish.
If an error is printed out, it will be shown in a message dialog.
'''
error = Signal(str, str, str)
'''@ivar: the signal is emitted if error output was detected (id, decription, message)'''
finished = Signal(str)
'''@ivar: the signal is emitted on exit (id)'''
def __init__(self, args, bufsize=0, executable=None, stdin=None, stdout=None,
stderr=subprocess.PIPE, preexec_fn=None, close_fds=False,
shell=False, cwd=None, env=None, universal_newlines=False,
startupinfo=None, creationflags=0, object_id='', description=''):
'''
For arguments see https://docs.python.org/2/library/subprocess.html
Additional arguments:
:param object_id: the identification string of this object and title of the
error message dialog
:type object_id: str
:param description: the description string used as addiotional information
in dialog if an error was occured
:type description: str
'''
try:
QObject.__init__(self)
self._args = args
self._object_id = object_id
self._description = description
self.error.connect(self.on_error)
# wait for process to avoid 'defunct' processes
self.popen = subprocess.Popen(args=args, bufsize=bufsize, executable=executable, stdin=stdin, stdout=stdout,
stderr=stderr, preexec_fn=preexec_fn, close_fds=close_fds, shell=shell, cwd=cwd, env=env,
universal_newlines=universal_newlines, startupinfo=startupinfo, creationflags=creationflags)
thread = threading.Thread(target=self._supervise)
thread.setDaemon(True)
thread.start()
except Exception as _:
raise
# def __del__(self):
# print "Deleted:", self._description
@property
def stdout(self):
return self.popen.stdout
@property
def stderr(self):
return self.popen.stderr
@property
def stdin(self):
return self.popen.stdin
def _supervise(self):
'''
Wait for process to avoid 'defunct' processes
'''
self.popen.wait()
result_err = ''
if self.stderr is not None:
result_err = self.stderr.read()
if result_err:
self.error.emit(self._object_id, self._description, result_err)
self.finished.emit(self._object_id)
def on_error(self, object_id, descr, msg):
WarningMessageBox(QMessageBox.Warning, object_id, '%s\n\n'
'%s' % (descr, msg), ' '.join(self._args)).exec_()
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 NodeSelectorWidget(QWidget):
_COL_NAMES = ['Node']
# public signal
sig_node_selected = Signal(DynreconfClientWidget)
def __init__(self, parent, rospack, signal_msg=None):
"""
@param signal_msg: Signal to carries a system msg that is shown on GUI.
@type signal_msg: QtCore.Signal
"""
super(NodeSelectorWidget, self).__init__()
self._parent = parent
self.stretch = None
self._signal_msg = signal_msg
ui_file = os.path.join(rospack.get_path('rqt_reconfigure'), 'resource',
'node_selector.ui')
loadUi(ui_file, self)
# List of the available nodes. Since the list should be updated over
# time and we don't want to create node instance per every update
# cycle, This list instance should better be capable of keeping track.
self._nodeitems = OrderedDict()
# Dictionary. 1st elem is node's GRN name,
# 2nd is TreenodeQstdItem instance.
# TODO: Needs updated when nodes list updated.
# Setup treeview and models
self._item_model = TreenodeItemModel()
self._rootitem = self._item_model.invisibleRootItem() # QStandardItem
self._nodes_previous = None
# Calling this method updates the list of the node.
# Initially done only once.
self._update_nodetree_pernode()
# TODO(Isaac): Needs auto-update function enabled, once another
# function that updates node tree with maintaining
# collapse/expansion state. http://goo.gl/GuwYp can be a
# help.
self._collapse_button.pressed.connect(
self._node_selector_view.collapseAll)
self._expand_button.pressed.connect(self._node_selector_view.expandAll)
self._refresh_button.pressed.connect(self._refresh_nodes)
# Filtering preparation.
self._proxy_model = FilterChildrenModel(self)
self._proxy_model.setDynamicSortFilter(True)
self._proxy_model.setSourceModel(self._item_model)
self._node_selector_view.setModel(self._proxy_model)
self._filterkey_prev = ''
# This 1 line is needed to enable horizontal scrollbar. This setting
# isn't available in .ui file.
# Ref. http://stackoverflow.com/a/6648906/577001
try:
self._node_selector_view.header().setResizeMode(
0, QHeaderView.ResizeToContents) # Qt4
except AttributeError:
# TODO QHeaderView.setSectionResizeMode() is currently segfaulting
# using Qt 5 with both bindings PyQt as well as PySide
pass
# Setting slot for when user clicks on QTreeView.
self.selectionModel = self._node_selector_view.selectionModel()
# Note: self.selectionModel.currentChanged doesn't work to deselect
# a treenode as expected. Need to use selectionChanged.
self.selectionModel.selectionChanged.connect(
self._selection_changed_slot)
def node_deselected(self, grn):
"""
Deselect the index that corresponds to the given GRN.
:type grn: str
"""
# Obtain the corresponding index.
qindex_tobe_deselected = self._item_model.get_index_from_grn(grn)
rospy.logdebug('NodeSelWidt node_deselected qindex={} data={}'.format(
qindex_tobe_deselected,
qindex_tobe_deselected.data(Qt.DisplayRole)))
# Obtain all indices currently selected.
indexes_selected = self.selectionModel.selectedIndexes()
for index in indexes_selected:
grn_from_selectedindex = RqtRosGraph.get_upper_grn(index, '')
rospy.logdebug(' Compare given grn={} grn from selected={}'.format(
grn, grn_from_selectedindex))
# If GRN retrieved from selected index matches the given one.
if grn == grn_from_selectedindex:
# Deselect the index.
self.selectionModel.select(index, QItemSelectionModel.Deselect)
def node_selected(self, grn):
"""
Select the index that corresponds to the given GRN.
:type grn: str
"""
# Obtain the corresponding index.
qindex_tobe_selected = self._item_model.get_index_from_grn(grn)
rospy.logdebug('NodeSelWidt node_selected qindex={} data={}'.format(
qindex_tobe_selected, qindex_tobe_selected.data(Qt.DisplayRole)))
# Select the index.
if qindex_tobe_selected:
self.selectionModel.select(qindex_tobe_selected,
QItemSelectionModel.Select)
def _selection_deselected(self, index_current, rosnode_name_selected):
"""
Intended to be called from _selection_changed_slot.
"""
self.selectionModel.select(index_current, QItemSelectionModel.Deselect)
try:
reconf_widget = self._nodeitems[
rosnode_name_selected].get_dynreconf_widget()
except ROSException as e:
raise e
# Signal to notify other pane that also contains node widget.
self.sig_node_selected.emit(reconf_widget)
#self.sig_node_selected.emit(self._nodeitems[rosnode_name_selected])
def _selection_selected(self, index_current, rosnode_name_selected):
"""Intended to be called from _selection_changed_slot."""
rospy.logdebug('_selection_changed_slot row={} col={} data={}'.format(
index_current.row(), index_current.column(),
index_current.data(Qt.DisplayRole)))
# Determine if it's terminal treenode.
found_node = False
for nodeitem in self._nodeitems.values():
name_nodeitem = nodeitem.data(Qt.DisplayRole)
name_rosnode_leaf = rosnode_name_selected[
rosnode_name_selected.rfind(RqtRosGraph.DELIM_GRN) + 1:]
# If name of the leaf in the given name & the name taken from
# nodeitem list matches.
if ((name_nodeitem == rosnode_name_selected) and
(name_nodeitem[name_nodeitem.rfind(RqtRosGraph.DELIM_GRN) + 1:]
== name_rosnode_leaf)):
rospy.logdebug('terminal str {} MATCH {}'.format(
name_nodeitem, name_rosnode_leaf))
found_node = True
break
if not found_node: # Only when it's NOT a terminal we deselect it.
self.selectionModel.select(index_current,
QItemSelectionModel.Deselect)
return
# Only when it's a terminal we move forward.
item_child = self._nodeitems[rosnode_name_selected]
item_widget = None
try:
item_widget = item_child.get_dynreconf_widget()
except ROSException as e:
raise e
rospy.logdebug('item_selected={} child={} widget={}'.format(
index_current, item_child, item_widget))
self.sig_node_selected.emit(item_widget)
# Show the node as selected.
#selmodel.select(index_current, QItemSelectionModel.SelectCurrent)
def _selection_changed_slot(self, selected, deselected):
"""
Sends "open ROS Node box" signal ONLY IF the selected treenode is the
terminal treenode.
Receives args from signal QItemSelectionModel.selectionChanged.
:param selected: All indexs where selected (could be multiple)
:type selected: QItemSelection
:type deselected: QItemSelection
"""
## Getting the index where user just selected. Should be single.
if not selected.indexes() and not deselected.indexes():
rospy.logerr('Nothing selected? Not ideal to reach here')
return
index_current = None
if selected.indexes():
index_current = selected.indexes()[0]
elif len(deselected.indexes()) == 1:
# Setting length criteria as 1 is only a workaround, to avoid
# Node boxes on right-hand side disappears when filter key doesn't
# match them.
# Indeed this workaround leaves another issue. Question for
# permanent solution is asked here http://goo.gl/V4DT1
index_current = deselected.indexes()[0]
rospy.logdebug(' - - - index_current={}'.format(index_current))
rosnode_name_selected = RqtRosGraph.get_upper_grn(index_current, '')
# If retrieved node name isn't in the list of all nodes.
if not rosnode_name_selected in self._nodeitems.keys():
# De-select the selected item.
self.selectionModel.select(index_current,
QItemSelectionModel.Deselect)
return
if selected.indexes():
try:
self._selection_selected(index_current, rosnode_name_selected)
except ROSException as e:
#TODO: print to sysmsg pane
err_msg = e.message + '. Connection to node=' + \
format(rosnode_name_selected) + ' failed'
self._signal_msg.emit(err_msg)
rospy.logerr(err_msg)
elif deselected.indexes():
try:
self._selection_deselected(index_current,
rosnode_name_selected)
except ROSException as e:
rospy.logerr(e.message)
#TODO: print to sysmsg pane
def get_paramitems(self):
"""
:rtype: OrderedDict 1st elem is node's GRN name,
2nd is TreenodeQstdItem instance
"""
return self._nodeitems
def _update_nodetree_pernode(self):
"""
"""
# TODO(Isaac): 11/25/2012 dynamic_reconfigure only returns params that
# are associated with nodes. In order to handle independent
# params, different approach needs taken.
try:
nodes = dyn_reconf.find_reconfigure_services()
except rosservice.ROSServiceIOException as e:
rospy.logerr("Reconfigure GUI cannot connect to master.")
raise e # TODO Make sure 'raise' here returns or finalizes func.
if not nodes == self._nodes_previous:
i_node_curr = 1
num_nodes = len(nodes)
elapsedtime_overall = 0.0
for node_name_grn in nodes:
# Skip this grn if we already have it
if node_name_grn in self._nodeitems:
i_node_curr += 1
continue
time_siglenode_loop = time.time()
####(Begin) For DEBUG ONLY; skip some dynreconf creation
# if i_node_curr % 2 != 0:
# i_node_curr += 1
# continue
#### (End) For DEBUG ONLY. ####
# Instantiate QStandardItem. Inside, dyn_reconf client will
# be generated too.
treenodeitem_toplevel = TreenodeQstdItem(
node_name_grn, TreenodeQstdItem.NODE_FULLPATH)
_treenode_names = treenodeitem_toplevel.get_treenode_names()
# Using OrderedDict here is a workaround for StdItemModel
# not returning corresponding item to index.
self._nodeitems[node_name_grn] = treenodeitem_toplevel
self._add_children_treenode(treenodeitem_toplevel,
self._rootitem, _treenode_names)
time_siglenode_loop = time.time() - time_siglenode_loop
elapsedtime_overall += time_siglenode_loop
_str_progress = 'reconf ' + \
'loading #{}/{} {} / {}sec node={}'.format(
i_node_curr, num_nodes, round(time_siglenode_loop, 2),
round(elapsedtime_overall, 2), node_name_grn)
# NOT a debug print - please DO NOT remove. This print works
# as progress notification when loading takes long time.
rospy.logdebug(_str_progress)
i_node_curr += 1
def _add_children_treenode(self, treenodeitem_toplevel,
treenodeitem_parent, child_names_left):
"""
Evaluate current treenode and the previous treenode at the same depth.
If the name of both nodes is the same, current node instance is
ignored (that means children will be added to the same parent). If not,
the current node gets added to the same parent node. At the end, this
function gets called recursively going 1 level deeper.
:type treenodeitem_toplevel: TreenodeQstdItem
:type treenodeitem_parent: TreenodeQstdItem.
:type child_names_left: List of str
:param child_names_left: List of strings that is sorted in hierarchical
order of params.
"""
# TODO(Isaac): Consider moving this method to rqt_py_common.
name_currentnode = child_names_left.pop(0)
grn_curr = treenodeitem_toplevel.get_raw_param_name()
stditem_currentnode = TreenodeQstdItem(grn_curr,
TreenodeQstdItem.NODE_FULLPATH)
# item at the bottom is your most recent node.
row_index_parent = treenodeitem_parent.rowCount() - 1
# Obtain and instantiate prev node in the same depth.
name_prev = ''
stditem_prev = None
if treenodeitem_parent.child(row_index_parent):
stditem_prev = treenodeitem_parent.child(row_index_parent)
name_prev = stditem_prev.text()
stditem = None
# If the name of both nodes is the same, current node instance is
# ignored (that means children will be added to the same parent)
if name_prev != name_currentnode:
stditem_currentnode.setText(name_currentnode)
# Arrange alphabetically by display name
insert_index = 0
while insert_index < treenodeitem_parent.rowCount(
) and treenodeitem_parent.child(
insert_index).text() < name_currentnode:
insert_index += 1
treenodeitem_parent.insertRow(insert_index, stditem_currentnode)
stditem = stditem_currentnode
else:
stditem = stditem_prev
if child_names_left:
# TODO: Model is closely bound to a certain type of view (treeview)
# here. Ideally isolate those two. Maybe we should split into 2
# class, 1 handles view, the other does model.
self._add_children_treenode(treenodeitem_toplevel, stditem,
child_names_left)
else: # Selectable ROS Node.
#TODO: Accept even non-terminal treenode as long as it's ROS Node.
self._item_model.set_item_from_index(grn_curr, stditem.index())
def _prune_nodetree_pernode(self):
try:
nodes = dyn_reconf.find_reconfigure_services()
except rosservice.ROSServiceIOException as e:
rospy.logerr("Reconfigure GUI cannot connect to master.")
raise e # TODO Make sure 'raise' here returns or finalizes func.
for i in reversed(range(0, self._rootitem.rowCount())):
candidate_for_removal = self._rootitem.child(
i).get_raw_param_name()
if not candidate_for_removal in nodes:
rospy.logdebug(
'Removing {} because the server is no longer available.'.
format(candidate_for_removal))
self._nodeitems[candidate_for_removal].disconnect_param_server(
)
self._rootitem.removeRow(i)
self._nodeitems.pop(candidate_for_removal)
def _refresh_nodes(self):
self._prune_nodetree_pernode()
self._update_nodetree_pernode()
def close_node(self):
rospy.logdebug(" in close_node")
# TODO(Isaac) Figure out if dynamic_reconfigure needs to be closed.
def set_filter(self, filter_):
"""
Pass fileter instance to the child proxymodel.
:type filter_: BaseFilter
"""
self._proxy_model.set_filter(filter_)
def _test_sel_index(self, selected, deselected):
"""
Method for Debug only
"""
#index_current = self.selectionModel.currentIndex()
src_model = self._item_model
index_current = None
index_deselected = None
index_parent = None
curr_qstd_item = None
if selected.indexes():
index_current = selected.indexes()[0]
index_parent = index_current.parent()
curr_qstd_item = src_model.itemFromIndex(index_current)
elif deselected.indexes():
index_deselected = deselected.indexes()[0]
index_parent = index_deselected.parent()
curr_qstd_item = src_model.itemFromIndex(index_deselected)
if selected.indexes() > 0:
rospy.logdebug('sel={} par={} desel={} sel.d={} par.d={}'.format(
index_current,
index_parent,
index_deselected,
index_current.data(Qt.DisplayRole),
index_parent.data(Qt.DisplayRole),
) + ' desel.d={} cur.item={}'.format(
None, # index_deselected.data(Qt.DisplayRole)
curr_qstd_item))
elif deselected.indexes():
rospy.logdebug(
'sel={} par={} desel={} sel.d={} par.d={}'.format(
index_current, index_parent, index_deselected, None,
index_parent.data(Qt.DisplayRole)) +
' desel.d={} cur.item={}'.format(
index_deselected.data(Qt.DisplayRole), curr_qstd_item))
class ItemDelegate(QStyledItemDelegate):
'''
This ItemDelegate provides editors for different setting types in settings view.
'''
settings_path_changed_signal = Signal()
reload_settings = False
def createEditor(self, parent, option, index):
'''
Creates a editor in the TreeView depending on type of the settings data.
'''
item = self._itemFromIndex(index)
if item.edit_type() == SettingsValueItem.EDIT_TYPE_AUTODETECT:
if isinstance(item.value(), bool):
box = QCheckBox(parent)
box.setFocusPolicy(Qt.StrongFocus)
box.setAutoFillBackground(True)
box.stateChanged.connect(self.edit_finished)
return box
elif isinstance(item.value(), int):
box = QSpinBox(parent)
box.setValue(item.value())
if not item.value_min() is None:
box.setMinimum(item.value_min())
if not item.value_max() is None:
box.setMaximum(item.value_max())
if not item.value_step() is None:
box.setSingleStep(item.value_step())
return box
elif isinstance(item.value(), float):
box = QDoubleSpinBox(parent)
box.setValue(item.value())
if not item.value_min() is None:
box.setMinimum(item.value_min())
if not item.value_max() is None:
box.setMaximum(item.value_max())
if not item.value_step() is None:
box.setSingleStep(item.value_step())
box.setDecimals(3)
return box
elif item.edit_type() == SettingsValueItem.EDIT_TYPE_FOLDER:
editor = PathEditor(item.value(), parent)
editor.editing_finished_signal.connect(self.edit_finished)
return editor
elif item.edit_type() == SettingsValueItem.EDIT_TYPE_LIST:
box = QComboBox(parent)
box.addItems(item.value_list())
index = box.findText(item.value())
if index >= 0:
box.setCurrentIndex(index)
box.setEditable(False)
return box
return QStyledItemDelegate.createEditor(self, parent, option, index)
# def setEditorData(self, editor, index):
# print "setEditorData"
# QStyledItemDelegate.setEditorData(self, editor, index)
# def updateEditorGeometry(self, editor, option, index):
# print "updateEditorGeometry", option.rect.width()
# editor.setMaximumSize(option.rect.width(), option.rect.height())
# QStyledItemDelegate.updateEditorGeometry(self, editor, option, index)
def setModelData(self, editor, model, index):
if isinstance(editor, PathEditor):
cfg_path = nm.settings().cfg_path
model.setData(index, editor.path)
self.reload_settings = (cfg_path != nm.settings().cfg_path)
else:
QStyledItemDelegate.setModelData(self, editor, model, index)
def sizeHint(self, option, index):
'''
Determines and returns the size of the text after the format.
@see: U{http://www.pyside.org/docs/pyside/PySide/QtGui/QAbstractItemDelegate.html#PySide.QtGui.QAbstractItemDelegate}
'''
options = QStyleOptionViewItem(option)
self.initStyleOption(options, index)
return QSize(options.rect.width(), 25)
def edit_finished(self, arg=None):
editor = self.sender()
# The commitData signal must be emitted when we've finished editing
# and need to write our changed back to the model.
self.commitData.emit(editor)
self.closeEditor.emit(editor, QAbstractItemDelegate.NoHint)
if self.reload_settings:
self.reload_settings = False
self.settings_path_changed_signal.emit()
def _itemFromIndex(self, index):
if isinstance(index.model(), QSortFilterProxyModel):
sindex = index.model().mapToSource(index)
return index.model().sourceModel().itemFromIndex(sindex)
else:
return index.model().itemFromIndex(index)
class GroupWidget(QWidget):
"""
(Isaac's guess as of 12/13/2012)
This class bonds multiple Editor instances that are associated with
a single node as a group.
"""
# public signal
sig_node_disabled_selected = Signal(str)
sig_node_state_change = Signal(bool)
def __init__(self, updater, config, nodename):
"""
:param config:
:type config: Dictionary? defined in dynamic_reconfigure.client.Client
:type nodename: str
"""
super(GroupWidget, self).__init__()
self.state = config['state']
self.param_name = config['name']
self._toplevel_treenode_name = nodename
# TODO: .ui file needs to be back into usage in later phase.
# ui_file = os.path.join(rp.get_path('rqt_reconfigure'),
# 'resource', 'singlenode_parameditor.ui')
# loadUi(ui_file, self)
verticalLayout = QVBoxLayout(self)
verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))
_widget_nodeheader = QWidget()
_h_layout_nodeheader = QHBoxLayout(_widget_nodeheader)
_h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))
self.nodename_qlabel = QLabel(self)
font = QFont('Trebuchet MS, Bold')
font.setUnderline(True)
font.setBold(True)
# Button to close a node.
_icon_disable_node = QIcon.fromTheme('window-close')
_bt_disable_node = QPushButton(_icon_disable_node, '', self)
_bt_disable_node.setToolTip('Hide this node')
_bt_disable_node_size = QSize(36, 24)
_bt_disable_node.setFixedSize(_bt_disable_node_size)
_bt_disable_node.pressed.connect(self._node_disable_bt_clicked)
_h_layout_nodeheader.addWidget(self.nodename_qlabel)
_h_layout_nodeheader.addWidget(_bt_disable_node)
self.nodename_qlabel.setAlignment(Qt.AlignCenter)
font.setPointSize(10)
self.nodename_qlabel.setFont(font)
grid_widget = QWidget(self)
self.grid = QFormLayout(grid_widget)
verticalLayout.addWidget(_widget_nodeheader)
verticalLayout.addWidget(grid_widget, 1)
# Again, these UI operation above needs to happen in .ui file.
self.tab_bar = None # Every group can have one tab bar
self.tab_bar_shown = False
self.updater = updater
self.editor_widgets = []
self._param_names = []
self._create_node_widgets(config)
logging.debug('Groups node name={}'.format(nodename))
self.nodename_qlabel.setText(nodename)
# Labels should not stretch
# self.grid.setColumnStretch(1, 1)
# self.setLayout(self.grid)
def collect_paramnames(self, config):
pass
def _create_node_widgets(self, config):
"""
:type config: Dict?
"""
i_debug = 0
for param in config['parameters']:
begin = time.time() * 1000
editor_type = '(none)'
if param['edit_method']:
widget = EnumEditor(self.updater, param)
elif param['type'] in EDITOR_TYPES:
logging.debug('GroupWidget i_debug={} param type ={}'.format(
i_debug, param['type']))
editor_type = EDITOR_TYPES[param['type']]
widget = eval(editor_type)(self.updater, param)
self.editor_widgets.append(widget)
self._param_names.append(param['name'])
logging.debug(
'groups._create_node_widgets num editors={}'.format(i_debug))
end = time.time() * 1000
time_elap = end - begin
logging.debug('ParamG editor={} loop=#{} Time={}msec'.format(
editor_type, i_debug, time_elap))
i_debug += 1
for name, group in sorted(config['groups'].items()):
if group['type'] == 'tab':
widget = TabGroup(self, self.updater, group,
self._toplevel_treenode_name)
elif group['type'] in _GROUP_TYPES.keys():
widget = eval(_GROUP_TYPES[group['type']])(
self.updater, group, self._toplevel_treenode_name)
else:
widget = eval(_GROUP_TYPES[''])(self.updater, group,
self._toplevel_treenode_name)
self.editor_widgets.append(widget)
logging.debug('groups._create_node_widgets name={}'.format(name))
for i, ed in enumerate(self.editor_widgets):
ed.display(self.grid)
logging.debug('GroupWdgt._create_node_widgets'
' len(editor_widgets)={}'.format(len(
self.editor_widgets)))
def display(self, grid):
grid.addRow(self)
def update_group(self, config):
if 'state' in config:
old_state = self.state
self.state = config['state']
if self.state != old_state:
self.sig_node_state_change.emit(self.state)
names = [name for name in config.keys()]
for widget in self.editor_widgets:
if isinstance(widget, EditorWidget):
if widget.param_name in names:
widget.update_value(config[widget.param_name])
elif isinstance(widget, GroupWidget):
cfg = find_cfg(config, widget.param_name) or config
widget.update_group(cfg)
def close(self):
for w in self.editor_widgets:
w.close()
def get_treenode_names(self):
"""
:rtype: str[]
"""
return self._param_names
def _node_disable_bt_clicked(self):
logging.debug('param_gs _node_disable_bt_clicked')
self.sig_node_disabled_selected.emit(self._toplevel_treenode_name)
class LaunchWidget(QDialog):
'''#TODO: comment
'''
# To be connected to PluginContainerWidget
sig_sysmsg = Signal(str)
def __init__(self, parent):
'''
@type parent: LaunchMain
'''
super(LaunchWidget, self).__init__()
self._parent = parent
self._config = None
#TODO: should be configurable from gui
self._port_roscore = 11311
self._run_id = None
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_launch'),
'resource', 'launch_widget.ui')
loadUi(ui_file, self)
# row=0 allows any number of rows to be added.
self._datamodel = QStandardItemModel(0, 1)
master_uri = rosenv.get_master_uri()
rospy.loginfo('LaunchWidget master_uri={}'.format(master_uri))
self._delegate = NodeDelegate(master_uri, self._rospack)
self._treeview.setModel(self._datamodel)
self._treeview.setItemDelegate(self._delegate)
# NodeController used in controller class for launch operation.
self._node_controllers = []
self._pushbutton_start_all.clicked.connect(self._parent.start_all)
self._pushbutton_stop_all.clicked.connect(self._parent.stop_all)
# Bind package selection with .launch file selection.
self._combobox_pkg.currentIndexChanged[str].connect(
self._refresh_launchfiles)
# Bind a launch file selection with launch GUI generation.
self._combobox_launchfile_name.currentIndexChanged[str].connect(
self._load_launchfile_slot)
self._update_pkgs_contain_launchfiles()
# Used for removing previous nodes
self._num_nodes_previous = 0
def _load_launchfile_slot(self, launchfile_name):
# Not yet sure why, but everytime combobox.currentIndexChanged occurs,
# this method gets called twice with launchfile_name=='' in 1st call.
if launchfile_name == None or launchfile_name == '':
return
_config = None
rospy.logdebug(
'_load_launchfile_slot launchfile_name={}'.format(launchfile_name))
try:
_config = self._create_launchconfig(launchfile_name,
self._port_roscore)
#TODO: folder_name_launchfile should be able to specify arbitrarily
# _create_launchconfig takes 3rd arg for it.
except IndexError as e:
msg = 'IndexError={} launchfile={}'.format(e.message,
launchfile_name)
rospy.logerr(msg)
self.sig_sysmsg.emit(msg)
return
except RLException as e:
msg = 'RLException={} launchfile={}'.format(
e.message, launchfile_name)
rospy.logerr(msg)
self.sig_sysmsg.emit(msg)
return
self._create_widgets_for_launchfile(_config)
def _create_launchconfig(self,
launchfile_name,
port_roscore=11311,
folder_name_launchfile='launch'):
'''
@rtype: ROSLaunchConfig
@raises RLException: raised by roslaunch.config.load_config_default.
'''
pkg_name = self._combobox_pkg.currentText()
try:
launchfile = os.path.join(self._rospack.get_path(pkg_name),
folder_name_launchfile, launchfile_name)
except IndexError as e:
raise RLException('IndexError: {}'.format(e.message))
try:
launch_config = roslaunch.config.load_config_default([launchfile],
port_roscore)
except rospkg.common.ResourceNotFound as e:
raise RLException('ResourceNotFound: {}'.format(e.message))
except RLException as e:
raise e
return launch_config
def _create_widgets_for_launchfile(self, config):
self._config = config
# Delete old nodes' GUIs.
self._node_controllers = []
# These lines seem to remove indexWidgets previously set on treeview.
# Per suggestion in API doc, we are not using reset(). Instead,
# using 2 methods below without any operation in between, which
# I suspect might be inproper.
# http://qt-project.org/doc/qt-4.8/qabstractitemmodel.html#reset
self._datamodel.beginResetModel()
self._datamodel.endResetModel()
# Need to store the num of nodes outside of the loop -- this will
# be used for removing excessive previous node rows.
order_xmlelement = 0
# Loop per xml element
for order_xmlelement, node in enumerate(self._config.nodes):
proxy = NodeProxy(self._run_id, self._config.master.uri, node)
# TODO: consider using QIcon.fromTheme()
status_label = StatusIndicator()
qindex_nodewidget = self._datamodel.index(order_xmlelement, 0,
QModelIndex())
node_widget = self._delegate.create_node_widget(
qindex_nodewidget, proxy.config, status_label)
#TODO: Ideally find a way so that we don't need this block.
#BEGIN If these lines are missing, widget won't be shown either.
std_item = QStandardItem(
#node_widget.get_node_name()
)
self._datamodel.setItem(order_xmlelement, 0, std_item)
#END If these lines are missing, widget won't be shown either.
self._treeview.setIndexWidget(qindex_nodewidget, node_widget)
node_controller = NodeController(proxy, node_widget)
self._node_controllers.append(node_controller)
node_widget.connect_start_stop_button( \
node_controller.start_stop_slot)
rospy.logdebug(
'loop #%d proxy.config.namespace=%s ' + 'proxy.config.name=%s',
order_xmlelement, proxy.config.namespace, proxy.config.name)
self._num_nodes_previous = order_xmlelement
self._parent.set_node_controllers(self._node_controllers)
def _update_pkgs_contain_launchfiles(self):
'''
Inspired by rqt_msg.MessageWidget._update_pkgs_contain_launchfiles
'''
packages = sorted([
pkg_tuple[0]
for pkg_tuple in RqtRoscommUtil.iterate_packages('launch')
])
self._package_list = packages
rospy.logdebug('pkgs={}'.format(self._package_list))
self._combobox_pkg.clear()
self._combobox_pkg.addItems(self._package_list)
self._combobox_pkg.setCurrentIndex(0)
def _refresh_launchfiles(self, package=None):
'''
Inspired by rqt_msg.MessageWidget._refresh_msgs
'''
if package is None or len(package) == 0:
return
self._launchfile_instances = [] # Launch[]
#TODO: RqtRoscommUtil.list_files's 2nd arg 'subdir' should NOT be
# hardcoded later.
_launch_instance_list = RqtRoscommUtil.list_files(package, 'launch')
rospy.logdebug('_refresh_launches package={} instance_list={}'.format(
package, _launch_instance_list))
self._launchfile_instances = [
x.split('/')[1] for x in _launch_instance_list
]
self._combobox_launchfile_name.clear()
self._combobox_launchfile_name.addItems(self._launchfile_instances)
def save_settings(self, plugin_settings, instance_settings):
# instance_settings.set_value('splitter', self._splitter.saveState())
pass
def restore_settings(self, plugin_settings, instance_settings):
# if instance_settings.contains('splitter'):
# self._splitter.restoreState(instance_settings.value('splitter'))
# else:
# self._splitter.setSizes([100, 100, 200])
pass
class JointStatePublisherGui(QWidget):
sliderUpdateTrigger = Signal()
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.scrollable = QWidget()
self.gridlayout = QGridLayout()
self.scroll = QScrollArea()
self.scroll.setWidgetResizable(True)
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
joint_layout.addWidget(slider)
self.joint_map[name] = {
'slidervalue': 0,
'display': display,
'slider': slider,
'joint': joint
}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(
lambda event, name=name: self.onValueChangedOne(name))
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders),
self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.scrollable.setLayout(self.gridlayout)
self.scroll.setWidget(self.scrollable)
self.vlayout.addWidget(self.scroll)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
self.setLayout(self.vlayout)
def onValueChangedOne(self, name):
# A slider value was changed, but we need to change the joint_info metadata.
joint_info = self.joint_map[name]
joint_info['slidervalue'] = joint_info['slider'].value()
joint = joint_info['joint']
joint['position'] = self.sliderToValue(joint_info['slidervalue'],
joint)
joint_info['display'].setText("%.2f" % joint['position'])
@pyqtSlot()
def updateSliders(self):
self.update_sliders()
def update_sliders(self):
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(
joint['position'], joint)
joint_info['slider'].setValue(joint_info['slidervalue'])
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(joint['zero'], joint))
def reorggrid_event(self, event):
self.reorganize_grid(event)
def reorganize_grid(self, number_of_rows):
self.num_rows = number_of_rows
# Remove items from layout (won't destroy them!)
items = []
for pos in self.positions:
item = self.gridlayout.itemAtPosition(*pos)
items.append(item)
self.gridlayout.removeItem(item)
# Generate new positions for sliders and place them in their new spots
self.positions = self.generate_grid_positions(len(items),
self.num_rows)
for item, pos in zip(items, self.positions):
self.gridlayout.addLayout(item, *pos)
def generate_grid_positions(self, num_items, num_rows):
if num_rows == 0:
return []
positions = [
(y, x)
for x in range(int((math.ceil(float(num_items) / num_rows))))
for y in range(num_rows)
]
positions = positions[:num_items]
return positions
def randomize_event(self, event):
self.randomize()
def randomize(self):
rospy.loginfo("Randomizing")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(random.uniform(joint['min'], joint['max']),
joint))
def sliderUpdate(self, event):
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
self.update_sliders()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] -
joint['min'])
def sliderToValue(self, slider, joint):
pctvalue = slider / float(RANGE)
return joint['min'] + (joint['max'] - joint['min']) * pctvalue
class ClassName(QObject):
_update_task_delegates = Signal()
_update_endeffector_widget = Signal()
def __init__(self, context):
QObject.__init__(self, context)
self.setObjectName('ClassName')
self.reward_total = 0
self.execute_policy = False
self.record_button = False
self.save_button = False
self.plot_button = False
self.t = 0
self.pos_car1 = []
self.pos_car2 = []
self.trajectory_collector = []
self.listener = tf.TransformListener()
self.car1_goal = [38., 3.]
#self.car2_goal = [38., 3.]
self.car2_goal = [30., 32.]
# setup publisher
# car 1
self._examplePublisher = rospy.Publisher('/policy_gui/exampletopic',
Bool,
queue_size=10)
self.goal_client1 = actionlib.SimpleActionClient(
'/car1/move_base', MoveBaseAction)
self.goal_client1.wait_for_server()
self.velocity_service1_ = rospy.ServiceProxy(
'/car1/car_control/pomdp_velocity', ActionObservation)
#self.listener = tf.TransformListener()
# car 2
self._examplePublisher = rospy.Publisher('/policy_gui/exampletopic',
Bool,
queue_size=10)
self.goal_client2 = actionlib.SimpleActionClient(
'/car2/move_base', MoveBaseAction)
self.goal_client2.wait_for_server()
self.velocity_service2_ = rospy.ServiceProxy(
'/car2/car_control/pomdp_velocity', ActionObservation)
# self.listener = tf.TransformListener()
# setup services
#self.getObjects = rospy.ServiceProxy('worldmodel/get_object_model', GetObjectModel)
# setup subscribers
#self._worldmodelObjectsSubscriber = rospy.Subscriber("/worldmodel/objects", ObjectModel, self._on_worldmodel_objects)
#self._interactive_marker_endeffector = rospy.Subscriber("/interactive_marker_pose_control/feedback",InteractiveMarkerFeedback,
#self._on_interactive_marker_endeffector_pose,queue_size=1)
# setup action clients
#self._move_arm_client = actionlib.SimpleActionClient("/move_group", MoveGroupAction)
#setup tf listener
#self.tf_listener = TransformListener()
# setup main widget
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('policy_gui'), 'lib',
'ClassName.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('ClassNameUi')
#set connections
self._widget.start_button.pressed.connect(
self._on_start_button_pressed)
self._widget.setup_button.pressed.connect(
self._on_setup_button_pressed)
self._widget.record_button.pressed.connect(
self._on_record_button_pressed)
self._widget.save_button.pressed.connect(self._on_save_button_pressed)
self._widget.plot_button.pressed.connect(self._on_plot_button_pressed)
#getRobotJoints_button
# 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.timer = rospy.Timer(rospy.Duration(1), self.policy_loop)
#taurob_training_week = rospy.get_param('/taurob_training_week',False)
# connect Signal Slot
#self._update_task_delegates.connect(self._on_update_task_delegates)
# init stuff
# self.bla = 1
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 _on_joint_states(self,message):
# arm_joints =['arm_joint_0', 'arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4']
############################################################
######################### BUTTONS ##########################
############################################################
###################################
# Setup button (Set Goal to Cars) #
###################################
def _on_setup_button_pressed(self):
# should send two navigation goals
print(' Setup Button pressed, publishing msg')
# goal for first car
goal1 = MoveBaseGoal()
goal1.target_pose.header.frame_id = "/car1/map"
goal1.target_pose.header.stamp = rospy.Time.now()
goal1.target_pose.pose.position.x = self.car1_goal[0]
goal1.target_pose.pose.position.y = self.car1_goal[1]
goal1.target_pose.pose.orientation.z = 1.0
self.goal_client1.send_goal(goal1)
# goal for second car
goal2 = MoveBaseGoal()
goal2.target_pose.header.frame_id = "/car2/map"
goal2.target_pose.header.stamp = rospy.Time.now()
goal2.target_pose.pose.position.x = self.car2_goal[0]
goal2.target_pose.pose.position.y = self.car2_goal[1]
goal2.target_pose.pose.orientation.z = 1.0
self.goal_client2.send_goal(goal2)
time_now = rospy.Time(0)
(trans1, rot1) = self.listener.lookupTransform('/car1/base_link',
'/map', time_now)
(trans2, rot2) = self.listener.lookupTransform('/car2/base_link',
'/map', time_now)
self.pos_car1 = trans1
self.pos_car2 = trans2
print("car 1: ")
print(self.pos_car1)
print("car 2: ")
print(self.pos_car2)
#####################################################
# Compute policy button (action/observarion/reward) #
#####################################################
def _on_start_button_pressed(self):
# should call compute policy method and compute policy will give list of actions.
# Should execute one action after another (kinda loop). Before or together send
# velocity to another car
# self.compute_policy()
self.execute_policy = True
self.reward_total = 0
self.t = 0
req = ActionObservationRequest()
req.action = 5
res = self.velocity_service2_.call(req)
#print(' Start Button pressed, publishing msg')
########################
# Trajectory Recording #
########################
def _on_record_button_pressed(self):
# Should record trajectory
print('Recording trajectory')
# position = []
self.record_button = True
self.save_button = False
self.trajectory_collector = []
self.timer_position = rospy.Timer(rospy.Duration(10.0 / 30.0),
self.trajectory_recording)
#################################
# Trajectory Saving to the File #
#################################
def _on_save_button_pressed(self):
# Should record trajectory
self.record_button = False
self.save_button = True
print('Saving data to the file...')
path = "/home/linjuk/adda_ros/src/code_lina/trajectories"
if not os.path.exists(path):
os.makedirs(path)
self.save_positions(path)
self.save_button = False
self.trajectory_collector = []
# self.seq = self.trajectory_collector
# self.file = open(os.path.join(path, "linaFile.txt"), "w")
# self.file.write(self.trajectory_collector)
#######################
# Plotting Trajectory #
#######################
def _on_plot_button_pressed(self):
# Should plot trajectories
self.plot_button = True
print('Plotting Trajectories...')
############################################################
###################### END OF BUTTONS ######################
############################################################
############################################################
# helper function to generate random choice based on wieghts
############################################################
def random_action(self):
actions = [1] * 45 + [2] * 30 + [3] * 25
return random.choice(actions)
###############
# state mutator
###############
def update_state(self):
# TODO: at the moment this function uses initial values but it needs to be replaced with realtime values using the new service
# fetch values from service
# update self.pos_car1 and 2
# self.pos_car1 index => 0 = x, 1 = y, 2 = velocity, 3 = target.x, 4 = target.y
time_now = rospy.Time(0)
(trans1, rot1) = self.listener.lookupTransform('/map',
'/car1/base_link',
time_now)
(trans2, rot2) = self.listener.lookupTransform('/map',
'/car2/base_link',
time_now)
self.pos_car1 = trans1
self.pos_car2 = trans2
state = np.zeros((10, 1))
state[0] = self.pos_car1[0] # car1 x possition
state[1] = self.pos_car1[1] # car1 y possition
# state[2] = self.velocity_service1_.call(req) # car1 velocity NOT SURE ABOUT THIS
# state[3] = goal.target_pose.pose.position.x # car1 goal x NOT SURE ABOUT THIS
# state[4] = goal.target_pose.pose.position.y # car1 goal y NOT SURE ABOUT THIS
state[5] = self.pos_car2[0] # car2 x possition
state[6] = self.pos_car2[1] # car2 y possition
# state[7] = self.velocity_service2_.call(req) # car1 velocity NOT SURE ABOUT THIS
# state[8] = goal.target_pose.pose.position.x # car1 goal x NOT SURE ABOUT THIS
# state[9] = goal.target_pose.pose.position.y # car1 goal y NOT SURE ABOUT THIS
return state
###########################################
# random action logic inside compute policy
###########################################
def compute_action(self, state):
print('Compute action ...')
# TODO: decide the logic to determine the action based on state, right now its random action with weighted options
print('Generating a random action:')
action = self.random_action()
print(action)
return state, action
############################################
# logic for transition inside compute policy
############################################
def transition(self, state, action):
print('Transition ...')
# perform action suggested by compute_action
req = ActionObservationRequest()
req.action = action
res = self.velocity_service1_.call(req)
# wait for 3 seconds
# time.sleep(1.0)
# get new state
new_state = self.update_state()
# observe environment
observation = np.zeros((7, 1))
observation[0] = self.pos_car1[0] # car1 x possition
observation[1] = self.pos_car1[1] # car1 y possition
# observation[2] = self.velocity_service1_.call(req) # car1 velocity NOT SURE ABOUT THIS
observation[3] = self.pos_car2[0] # car2 x possition
observation[4] = self.pos_car2[1] # car2 y possition
# observation[5] = self.velocity_service2_.call(req) # car2 velocity NOT SURE ABOUT THIS
return new_state, observation
############################
# function for reward logic
############################
def reward(self, state, action, observation):
# Negative reward for the action that was made ( for all actions the same negative reward)
# Negative reward, if the car is to close to the other car (pre-defined range)
# Speed reward ???
# Positive reward, if the car is close to its goal (pre-defined range)
print('Computing reward:')
print('Observation.distance: ', observation[6])
print("car 1: ")
print(self.pos_car1)
print("car 2: ")
print(self.pos_car2)
dist_car = math.sqrt((state[0] - state[5])**2 +
(state[1] - state[6])**2)
dist_goal = math.sqrt((state[0] - self.car1_goal[0])**2 +
(state[1] - self.car1_goal[1])**2)
print("distance between cars", dist_car)
print('distance between goal and the car1', dist_goal)
if dist_car >= 10:
reward = 5
elif 10 > dist_car >= 5:
reward = 0
elif 5 > dist_car >= 2:
reward = -10
elif 2 > dist_car >= 0:
reward = -100
#print('Step reward: ', reward)
#print('Total reward: ', reward_total)
return reward
# compute action
# return observation + next state
#############################################
# compute policy loop that computes action,
# makes the transition and gives reward until
# goal is achieved
#############################################
def policy_loop(self, event):
if self.execute_policy:
self.t = self.t + 1
print('EXECUTING POLICY ', self.t)
current_state = self.update_state()
state_before_transition, action = self.compute_action(
current_state)
state_next_transition, observation = self.transition(
state_before_transition, action)
print('Reward total: ', self.reward_total)
iteration_reward = self.reward(state_next_transition, action,
observation)
print('Iteration reward total: ', iteration_reward)
self.reward_total += iteration_reward
# current_state = self.update_state()
# if self.pos_car1[0] >= goal_x and self.pos_car1[1] >= goal_y:
dist_goal1 = math.sqrt((self.pos_car1[0] - self.car1_goal[0])**2 +
(self.pos_car1[1] - self.car1_goal[1])**2)
dist_goal2 = math.sqrt((self.pos_car2[0] - self.car2_goal[0])**2 +
(self.pos_car2[1] - self.car2_goal[1])**2)
print('dist_goal: ', dist_goal1)
if dist_goal1 < 2:
req = ActionObservationRequest()
req.action = 4
res = self.velocity_service1_.call(req)
self.execute_policy = False
print('dist_goa2: ', dist_goal2)
if dist_goal2 < 2:
req = ActionObservationRequest()
req.action = 4
res = self.velocity_service2_.call(req)
self.execute_policy = False
def save_positions(self, path):
with open(
"{}/right_{}.csv".format(
path, time.strftime("%Y_%m_%d_%H_%M_%S",
time.localtime())), "w") as fp:
writer = csv.writer(fp, delimiter=",")
writer.writerow(["time", "x", "y", "z"])
for point in self.trajectory_collector:
writer.writerow(
[point[0], point[1][0], point[1][1], point[1][2]])
# [ 123123, [1, 2, 3] ]
def lookup_position(self):
try:
translation, rotation = self.listener.lookupTransform(
"/map", "/car2/base_link", rospy.Time(0))
self.pos_car2 = translation # [1, 2, 3]
self.trajectory_collector.append([time.time(), self.pos_car2])
except (tf.ExtrapolationException, tf.ConnectivityException,
tf.LookupException):
pass
def trajectory_recording(self, event):
if self.record_button:
self.lookup_position()
