class PropStatus(Plugin):
def __init__(self, context):
super(PropStatus, self).__init__(context)
self.setObjectName('Status')
self._widget = QWidget()
loadUi(os.path.join(uipath, 'propagatorstatus.ui'), self._widget)
context.add_widget(self._widget)
rospy.Subscriber('/thrusters/info',ThrusterInfo,motordriver_callback)
rospy.Subscriber('/skytraq_serial',SerialPacket,gps_callback)
rospy.Subscriber('/imu/data_raw',Imu,imu_callback)
rospy.Subscriber('/scan',LaserScan,lidar_callback)
rospy.Subscriber('/mv_bluefox_camera_node/image_raw',Image,camera_callback)
rospy.Timer(rospy.Duration(2),kill)
self._update_timer = QTimer(self._widget)
self._update_timer.timeout.connect(self._on_update)
self._update_timer.start(100)
def _on_update(self):
self._widget.findChild(QListWidget, 'list').clear()
for i in ['FR','FL','BR','BL','GPS','IMU','LIDAR','CAMERA']:
pItem = QListWidgetItem(i);
if i in active:
pItem.setBackground(Qt.green);
else:
pItem.setBackground(Qt.red);
self._widget.findChild(QListWidget, 'list').addItem(pItem)
class MicoButtonsWidget(QWidget):
def __init__(self, widget):
super(MicoButtonsWidget, self).__init__()
rospkg_pack = rospkg.RosPack()
ui_file = os.path.join(rospkg_pack.get_path('mico_controller'), 'resource', 'MicoButtons.ui')
loadUi(ui_file, self)
self.start_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/start', MicoStart)
self.stop_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/stop', MicoStop)
self.home_arm_srv = rospy.ServiceProxy('mico_arm_driver/in/home_arm', MicoHome)
self._updateTimer = QTimer(self)
self._updateTimer.timeout.connect(self.timeout_callback)
def start(self):
self._updateTimer.start(1000) # loop rate[ms]
def stop(self):
self._updateTimer.stop()
def timeout_callback(self):
pass
# rqt override
def save_settings(self, plugin_settings, instance_settings):
pass
# instance_settings.set_value('topic_name', self._topic_name)
def restore_settings(self, plugin_settings, instance_settings):
pass
# topic_name = instance_settings.value('topic_name')
# try:
# self._topic_name = eval(topic_name)
# except Exception:
# self._topic_name = self.TOPIC_NAME
def shutdown_plugin(self):
self.stop()
@Slot()
def on_qt_start_btn_clicked(self):
rospy.loginfo(self.start_arm_srv())
@Slot()
def on_qt_stop_btn_clicked(self):
rospy.loginfo(self.stop_arm_srv())
@Slot()
def on_qt_home_btn_clicked(self):
rospy.loginfo(self.home_arm_srv())
class RosPlot(FigureCanvas):
"""Ultimately, this is a QWidget (as well as a FigureCanvasAgg, etc.)."""
def __init__(self, parent, label_name,topic_name,topic_type,topic_field,buffer_size):
self.label_name= label_name
self.topic_type = topic_type
self.topic_name = topic_name
self.topic_field = topic_field
self.buffer_size = buffer_size
self.timer = QTimer()
self.timer.setInterval(100)
self.timer.timeout.connect(self.update_figure)
fig = Figure(figsize=(5, 4), dpi=100)
self.axes = fig.add_subplot(111)
# We want the axes cleared every time plot() is called
self.axes.hold(False)
self.compute_initial_figure()
self.buffer = collections.deque(maxlen=self.buffer_size)
FigureCanvas.__init__(self, fig)
self.setParent(parent)
FigureCanvas.setSizePolicy(self,
QtGui.QSizePolicy.Expanding,
QtGui.QSizePolicy.Expanding)
FigureCanvas.updateGeometry(self)
self.subscriber = RosHelper.create_subscriber_from_type(self.topic_name,self.topic_type,self.on_message)
self.timer.start()
def compute_initial_figure(self):
pass
def on_message(self,msg):
r = msg
for subfields in self.topic_field.split(".")[1:]:
r = getattr(r,subfields)
self.buffer.append(r)
def update_figure(self):
x = np.array(range(0,len(self.buffer)))
y = np.array(self.buffer)
self.axes.plot(x, y.T, 'r')
self.draw()
class ProblemViewerWidget(QWidget):
_problem_text = ""
def __init__(self, plugin=None):
super(ProblemViewerWidget, self).__init__()
# Create QWidget
ui_file = os.path.join(rospkg.RosPack().get_path('rosplan_rqt'), 'resource', 'problem_viewer.ui')
loadUi(ui_file, self)
self.setObjectName('ROSPlanProblemViewer')
self._plugin = plugin
rospy.Subscriber("/kcl_rosplan/problem", String, self.refresh_problem)
# init and start update timers
self._timer_set_problem = QTimer(self)
self._timer_set_problem.timeout.connect(self.set_problem)
self.start()
def start(self):
self._timer_set_problem.start(1000)
"""
updating problem view
"""
def refresh_problem(self, msg):
self._problem_text = msg.data
def set_problem(self):
self.textEdit.setPlainText(self._problem_text)
"""
Qt methods
"""
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class TopicDataWidget(DataDialog):
def __init__(self, plugin):
super(TopicDataWidget, self).__init__()
rospkgPack = rospkg.RosPack()
self._plugin = plugin
self._topicName = TOPIC_NAME
self._subscriber = None
self._updateTimer = QTimer(self)
self._updateTimer.timeout.connect(self.timeoutCallback)
self._updateTimer.start(40)
self.subscribeTopic(self._topicName)
# rqt override
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('topic_name', self._topicName)
def restore_settings(self, plugin_settings, instance_settings):
topicName = instance_settings.value('topic_name')
try:
self._topicName = eval(topicName)
except Exception:
topicName = None
def shutdown_plugin(self):
self.unregister_topic()
# subscribe topic
def subscribeTopic(self, topicName):
# msgClass, self._topicName, _ = get_topic_class(topicName)
self._subscriber = rospy.Subscriber(TOPIC_NAME, MSG_CLASS, self.messageCallback)
def messageCallback(self, msg):
self.setDisplayedData(msg.data, TOPIC_NAME)
def unregisterTopic(self):
if self._subscriber:
self._subscriber.unregister()
def timeoutCallback(self):
pass
class BaseFilter(QObject):
"""
Contains basic functions common to all filters.
Handles enabled logic and change notification.
"""
filter_changed_signal = Signal()
def __init__(self):
super(BaseFilter, self).__init__()
self._enabled = True
self._timer = QTimer(self)
self._timer.setSingleShot(True)
self._timer.timeout.connect(self.filter_changed_signal.emit)
def start_emit_timer(self, msec=None):
"""
Starts a timer to emit a signal to refresh the filters after the filter is changed
:param msec: number of msecs to wait before emitting the signal to change the filter ''int''
"""
if msec is None:
self._timer.start(10)
else:
self._timer.start(msec)
def is_enabled(self):
return self._enabled
def set_enabled(self, checked):
"""
Setter for _enabled
:param checked: boolean flag to set ''bool''
:emits filter_changed_signal: Always
"""
self._enabled = checked
self.start_emit_timer(200)
def has_filter(self):
raise NotImplementedError()
def test_message(self, message):
raise NotImplementedError()
class ParamCheckThread(threading.Thread):
def __init__(self, parent, signal_tobe_emitted, params_monitored):
"""
@type params_monitored: str[]
"""
super(ParamCheckThread, self).__init__()
self._parent = parent
self._signal = signal_tobe_emitted
self._params_monitored = params_monitored
self._timer = QTimer()
self._timer.timeout.connect(self._monitor_param)
def run(self):
self._timer.start(300)
def _monitor_param(self):
for param in self._params_monitored:
has_param = rospy.has_param(param)
self._signal.emit(has_param, param)
class NodeMonitorThread(threading.Thread):
def __init__(self, parent, signal_tobe_emitted, nodes_monitored):
"""
@type nodes_monitored: str[]
"""
super(NodeMonitorThread, self).__init__()
self._parent = parent
self._signal = signal_tobe_emitted
self._nodes_monitored = nodes_monitored
self._timer = QTimer()
self._timer.timeout.connect(self._monitor_nodes)
def run(self):
self._timer.start(500)
def _monitor_nodes(self):
for nodename in self._nodes_monitored:
is_node_running = rosnode_ping(nodename, 1)
#TODO: segfault occurs here most of every time the plugin shut down
self._signal.emit(is_node_running, nodename)
class Engineering_Plant(Plugin):
def __init__(self, context):
super(Engineering_Plant, self).__init__(context)
self.setObjectName('Engineering_Plant')
self._widget = QWidget()
loadUi(os.path.join(uipath, 'engineering_plant.ui'), self._widget)
context.add_widget(self._widget)
self._widget.findChild(QPushButton, 'Kill').clicked.connect(self.Kill)
self._widget.findChild(QPushButton, 'UnKill').clicked.connect(self.Un_kill)
rospy.Subscriber('/power_router/status',prstatus,data_update)
self.killservice = rospy.ServiceProxy('/power_router/setkill', SetKill)
self._update_timer = QTimer(self._widget)
self._update_timer.timeout.connect(self._on_update)
self._update_timer.start(1000)
def _on_update(self):
global computer_current, motor_current, temperature, battery
compratio = int(float(computer_current/300.0)*100)
self._widget.findChild(QProgressBar, 'ComputerCurrent').setValue(compratio)
motorratio = int(float(motor_current/9000.0)*100)
self._widget.findChild(QProgressBar, 'MotorCurrent').setValue(motorratio)
#tempratio = int(float(temperature/150)*100)
self._widget.findChild(QLCDNumber, 'tempLCD').display(temperature)
battratio = int(float(battery/100)*100)
self._widget.findChild(QProgressBar, 'Battery').setValue(battratio)
def Kill(self):
self.killservice(True)
def Un_kill(self):
self.killservice(False)
class EchoDialog(QDialog):
MESSAGE_CHARS_LIMIT = 1000
MESSAGE_LINE_LIMIT = 80
MESSAGE_HZ_LIMIT = 10
MAX_DISPLAY_MSGS = 25
STATISTIC_QUEUE_LEN = 1000
SHOW_BYTES = True
SHOW_JITTER = True
SHOW_STD_DEV = False
SHOW_WINDOW_SIZE = False
'''
This dialog shows the output of a topic.
'''
finished_signal = Signal(str)
'''
finished_signal has as parameter the name of the topic and is emitted, if this
dialog was closed.
'''
msg_signal = Signal(object, bool)
'''
msg_signal is a signal, which is emitted, if a new message was received.
'''
text_hz_signal = Signal(str)
text_signal = Signal(str)
'''
text_signal is a signal, which is emitted, if a new text to display was received.
'''
text_error_signal = Signal(str)
'''
text_error_signal is a signal, which is emitted, if a new error text to display was received.
'''
request_pw = Signal(object)
def __init__(self,
topic,
msg_type,
show_only_rate=False,
masteruri=None,
use_ssh=False,
parent=None):
'''
Creates an input dialog.
@param topic: the name of the topic
@type topic: C{str}
@param msg_type: the type of the topic
@type msg_type: C{str}
@raise Exception: if no topic class was found for the given type
'''
QDialog.__init__(self, parent=parent)
self._masteruri = masteruri
masteruri_str = '' if masteruri is None else '[%s]' % masteruri
echo_dialog_file = os.path.join(
os.path.dirname(os.path.realpath(__file__)), 'EchoDialog.ui')
loadUi(echo_dialog_file, self)
self.setObjectName(' - '.join(['EchoDialog', topic, masteruri_str]))
self.setAttribute(Qt.WA_DeleteOnClose, True)
self.setWindowFlags(Qt.Window)
self.setWindowTitle('%s %s %s' %
('Echo --- ' if not show_only_rate else 'Hz --- ',
topic, masteruri_str))
self.resize(900, 512)
self.topic = topic
self.show_only_rate = show_only_rate
self.lock = threading.RLock()
self.last_printed_count = 0
self.msg_t0 = -1.
self.msg_tn = 0
self.times = []
self.bytes = []
self.message_count = 0
self._state_message = ''
self._state_size_message = ''
self._scrapped_msgs = 0
self._scrapped_msgs_sl = 0
self._last_received_ts = 0
self.chars_limit = self.MESSAGE_CHARS_LIMIT
self.receiving_hz = self.MESSAGE_HZ_LIMIT
self.line_limit = self.MESSAGE_LINE_LIMIT
self.max_displayed_msgs = self.MAX_DISPLAY_MSGS
self.digits_after_in_array = 2
self.enabled_message_filter = True
self.field_filter_fn = None
self._latched = False
self._msgs = []
self.filterFrame.setVisible(False)
self.topicControlButton.clicked.connect(
self.on_topic_control_btn_clicked)
self.clearButton.clicked.connect(self.on_clear_btn_clicked)
if show_only_rate:
self.filterButton.setVisible(False)
else:
self.filterButton.clicked.connect(self.on_filter_clicked)
self.showStringsCheckBox.toggled.connect(
self.on_no_str_checkbox_toggled)
self.maxLenStringComboBox.activated[str].connect(
self.combobox_reduce_ch_activated)
self.showArraysCheckBox.toggled.connect(
self.on_no_arr_checkbox_toggled)
self.maxDigitsComboBox.activated[str].connect(
self.combobox_reduce_digits_activated)
self.enableMsgFilterCheckBox.toggled.connect(
self.on_enable_msg_filter_checkbox_toggled)
self.maxLenComboBox.activated[str].connect(
self.on_combobox_chars_activated)
self.maxHzComboBox.activated[str].connect(
self.on_combobox_hz_activated)
self.displayCountComboBox.activated[str].connect(
self.on_combobox_count_activated)
self.combobox_reduce_ch_activated(self.MESSAGE_LINE_LIMIT)
self.on_combobox_chars_activated(self.MESSAGE_CHARS_LIMIT)
self.on_combobox_hz_activated(self.MESSAGE_HZ_LIMIT)
self.on_combobox_count_activated(self.MAX_DISPLAY_MSGS)
self.filterButton.setFocus()
self.display.setReadOnly(True)
self.display.document().setMaximumBlockCount(500)
self._blocks_in_msg = None
self.display.setOpenLinks(False)
self.display.anchorClicked.connect(self._on_display_anchorClicked)
# subscribe to the topic
errmsg = ''
try:
self.__msg_class = message.get_message_class(msg_type)
if not self.__msg_class:
errmsg = "Cannot load message class for [%s]. Did you build messages?" % msg_type
except Exception as e:
self.__msg_class = None
errmsg = "Cannot load message class for [%s]. Did you build messagest?\nError: %s" % (
msg_type, utf8(e))
# variables for Subscriber
self.msg_signal.connect(self._append_message)
self.sub = None
# vairables for SSH connection
self.ssh_output_file = None
self.ssh_error_file = None
self.ssh_input_file = None
self.text_signal.connect(self._append_text)
self.text_hz_signal.connect(self._append_text_hz)
self._current_msg = ''
self._current_errmsg = ''
self.text_error_signal.connect(self._append_error_text)
# decide, which connection to open
if use_ssh:
self.__msg_class = None
self._on_display_anchorClicked(QUrl(self._masteruri))
elif self.__msg_class is None:
errtxt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">\n%s</pre>' % (
errmsg)
self.display.setText('<a href="%s">open using SSH</a>' %
(masteruri))
self.display.append(errtxt)
else:
self.sub = rospy.Subscriber(self.topic, self.__msg_class,
self._msg_handle)
self.print_hz_timer = QTimer()
self.print_hz_timer.timeout.connect(self._on_calc_hz)
self.print_hz_timer.start(1000)
self._start_time = time.time()
# print "======== create", self.objectName()
#
# def __del__(self):
# print "******* destroy", self.objectName()
# def hideEvent(self, event):
# self.close()
def closeEvent(self, event):
self.print_hz_timer.stop()
if self.sub is not None:
self.sub.unregister()
del self.sub
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
self.finished_signal.emit(self.topic)
if self.parent() is None:
QApplication.quit()
def create_field_filter(self, echo_nostr, echo_noarr):
def field_filter(val):
try:
# fields = val.__slots__
# field_types = val._slot_types
for f, t in zip(val.__slots__, val._slot_types):
if echo_noarr and '[' in t:
continue
elif echo_nostr and 'string' in t:
continue
yield f
except Exception:
pass
return field_filter
def on_filter_clicked(self, checked):
self.filterFrame.setVisible(checked)
def on_no_str_checkbox_toggled(self, state):
self.maxLenStringComboBox.setEnabled(state)
self.field_filter_fn = self.create_field_filter(
not state, not self.showArraysCheckBox.isChecked())
def on_no_arr_checkbox_toggled(self, state):
self.maxDigitsComboBox.setEnabled(state)
self.field_filter_fn = self.create_field_filter(
not self.showStringsCheckBox.isChecked(), not state)
def combobox_reduce_ch_activated(self, ch_txt):
try:
self.line_limit = int(ch_txt)
except ValueError:
try:
self.line_limit = float(ch_txt)
except ValueError:
self.maxLenStringComboBox.setEditText(str(self.line_limit))
self.display.clear()
with self.lock:
for msg, current_time in self._msgs:
self._append_message(msg, self._latched, current_time, False)
def combobox_reduce_digits_activated(self, ch_txt):
try:
self.digits_after_in_array = int(ch_txt)
except ValueError:
self.digits_after_in_array = None
self.maxDigitsComboBox.setEditText('')
self.display.clear()
with self.lock:
for msg, current_time in self._msgs:
self._append_message(msg, self._latched, current_time, False)
def on_enable_msg_filter_checkbox_toggled(self, state):
self.enabled_message_filter = state
self.maxLenComboBox.setEnabled(state)
self.maxHzComboBox.setEnabled(state)
if self.enabled_message_filter:
self.on_combobox_chars_activated(self.maxLenComboBox.currentText(),
False)
self.on_combobox_hz_activated(self.maxHzComboBox.currentText(),
False)
else:
self.chars_limit = 0
self.receiving_hz = 0
self.display.clear()
with self.lock:
for msg, current_time in self._msgs:
self._append_message(msg, self._latched, current_time, False)
def on_combobox_chars_activated(self, chars_txt, update_display=True):
if not self.enabled_message_filter:
return
try:
self.chars_limit = int(chars_txt)
except ValueError:
try:
self.chars_limit = float(chars_txt)
except ValueError:
self.maxLenComboBox.setEditText(str(self.chars_limit))
if update_display:
self.display.clear()
with self.lock:
for msg, current_time in self._msgs:
self._append_message(msg, self._latched, current_time,
False)
def on_combobox_hz_activated(self, hz_txt, update_display=True):
if not self.enabled_message_filter:
return
try:
self.receiving_hz = int(hz_txt)
except ValueError:
try:
self.receiving_hz = float(hz_txt)
except ValueError:
self.maxHzComboBox.setEditText(str(self.receiving_hz))
if update_display:
self.display.clear()
with self.lock:
for msg, current_time in self._msgs:
self._append_message(msg, self._latched, current_time,
False)
def on_combobox_count_activated(self, count_txt):
try:
self.max_displayed_msgs = int(count_txt)
self._blocks_in_msg = None
except ValueError:
self.displayCountComboBox.setEditText(str(self.max_displayed_msgs))
def on_clear_btn_clicked(self):
self.display.clear()
with self.lock:
del self._msgs[:]
self.message_count = 0
self._scrapped_msgs = 0
del self.times[:]
del self.bytes[:]
def on_topic_control_btn_clicked(self):
try:
if self.sub is None and self.ssh_output_file is None:
if self.__msg_class:
self.sub = rospy.Subscriber(self.topic, self.__msg_class,
self._msg_handle)
self._start_time = time.time()
else:
self._on_display_anchorClicked(QUrl(self._masteruri))
self.topicControlButton.setIcon(
QIcon(':/icons/sekkyumu_stop.png'))
else:
if self.sub is not None:
self.sub.unregister()
self.sub = None
elif self.ssh_output_file is not None:
self.ssh_output_file.close()
self.ssh_error_file.close()
self.ssh_output_file = None
self.topicControlButton.setIcon(
QIcon(':/icons/sekkyumu_play.png'))
except Exception as e:
rospy.logwarn('Error while stop/play echo for topic %s: %s' %
(self.topic, utf8(e)))
def _msg_handle(self, data):
self.msg_signal.emit(data,
(data._connection_header['latching'] != '0'))
def _append_message(self, msg, latched, current_time=None, store=True):
'''
Adds a label to the dialog's layout and shows the given text.
@param msg: the text to add to the dialog
@type msg: message object
'''
if current_time is None:
current_time = time.time()
self._latched = latched
if store:
with self.lock:
self._msgs.append((msg, current_time))
if len(self._msgs) > 25:
self._msgs.pop()
msg_len = -1
if (self.SHOW_BYTES or self.show_only_rate):
buff = None
try:
from cStringIO import StringIO # Python 2.x
buff = StringIO()
import os
msg.serialize(buff)
buff.seek(0, os.SEEK_END)
msg_len = buff.tell()
except ImportError:
from io import BytesIO # Python 3.x
buff = BytesIO()
msg.serialize(buff)
msg_len = buff.getbuffer().nbytes
self._count_messages(current_time, msg_len)
# skip messages, if they are received often then MESSAGE_HZ_LIMIT
if self._last_received_ts != 0 and self.receiving_hz != 0:
if current_time - self._last_received_ts < 1.0 / self.receiving_hz:
if (not latched or
(latched and current_time - self._start_time > 3.0)):
self._scrapped_msgs += 1
self._scrapped_msgs_sl += 1
return
self._last_received_ts = current_time
if not self.show_only_rate:
# convert message to string and reduce line width to current limit
msg = self.strify_message(
msg,
field_filter=self.field_filter_fn,
fixed_numeric_width=self.digits_after_in_array)
if isinstance(msg, tuple):
msg = msg[0]
msg = self._trim_width(msg)
msg = msg.replace('<', '<').replace('>', '>')
msg_cated = False
if self.chars_limit != 0 and len(msg) > self.chars_limit:
msg = msg[0:self.chars_limit]
msg_cated = True
# create a notification about scrapped messages
if self._scrapped_msgs_sl > 0:
txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">scrapped %s message because of Hz-settings</pre>' % self._scrapped_msgs_sl
self.display.append(txt)
self._scrapped_msgs_sl = 0
txt = '<pre style="background-color:#FFFCCC; color:#000000;font-family:Fixedsys,Courier; padding:10px;">---------- %s --------------------\n%s</pre>' % (
datetime.now().strftime("%d.%m.%Y %H:%M:%S.%f"), msg)
# set the count of the displayed messages on receiving the first message
self._update_max_msg_count(txt)
self.display.append(txt)
if msg_cated:
txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">message has been cut off</pre>'
self.display.append(txt)
if store:
self._print_status()
def _count_messages(self, ts=time.time(), msg_len=-1):
'''
Counts the received messages. Call this method only on receive message.
'''
current_time = ts
with self.lock:
# time reset
if self.msg_t0 < 0 or self.msg_t0 > current_time:
self.msg_t0 = current_time
self.msg_tn = current_time
self.times = []
self.bytes = []
else:
self.times.append(current_time - self.msg_tn)
if msg_len > -1:
self.bytes.append(msg_len)
self.msg_tn = current_time
# keep only statistics for the last 5000 messages so as not to run out of memory
if len(self.times) > self.STATISTIC_QUEUE_LEN:
self.times.pop(0)
if len(self.bytes) > self.STATISTIC_QUEUE_LEN:
self.bytes.pop(0)
self.message_count += 1
def _trim_width(self, msg):
'''
reduce line width to current limit
:param msg: the message
:type msg: str
:return: trimmed message
'''
result = msg
if self.line_limit != 0:
a = ''
for l in msg.splitlines():
a = a + (l if len(l) <= self.line_limit else
l[0:self.line_limit - 3] + '...') + '\n'
result = a
return result
def _update_max_msg_count(self, txt):
'''
set the count of the displayed messages on receiving the first message
:param txt: text of the message, which will be added to the document
:type txt: str
'''
if self._blocks_in_msg is None:
td = QTextDocument(txt)
self._blocks_in_msg = td.blockCount()
self.display.document().setMaximumBlockCount(
self._blocks_in_msg * self.max_displayed_msgs)
def _on_calc_hz(self):
if rospy.is_shutdown():
self.close()
return
if not self.show_only_rate and time.time() - self._last_received_ts > 1:
# create a notification about scrapped messages
if self._scrapped_msgs_sl > 0:
txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">scrapped %s message because of Hz-settings</pre>' % self._scrapped_msgs_sl
self._scrapped_msgs_sl = 0
self.display.append(txt)
if self.message_count == self.last_printed_count:
return
with self.lock:
message_rate = ''
message_bytes = ''
message_jitter = ''
message_window = ''
message_std_dev = ''
message_scrapped = ''
sum_times = sum(self.times)
if sum_times == 0:
sum_times = 1
if (self.SHOW_BYTES or self.show_only_rate) and self.bytes:
sum_bytes = sum(self.bytes)
avg = sum_bytes / len(self.bytes)
last = self.bytes[-1]
if avg != last:
message_bytes = "size[ last: %s, avg: %s ]" % (
self._normilize_size_print(last),
self._normilize_size_print(avg))
else:
message_bytes = "size: %s" % (
self._normilize_size_print(last))
byte_rate = float(sum_bytes) / float(sum_times)
message_bytes += " bw: %s/s" % (
self._normilize_size_print(byte_rate))
# the code from ROS rostopic
n = len(self.times)
if n < 2:
return
mean = sum_times / n
rate = 1. / mean if mean > 0. else 0
message_rate = "average rate: %.3f" % rate
# min and max
if self.SHOW_JITTER or self.show_only_rate:
max_delta = max(self.times)
min_delta = min(self.times)
message_jitter = "jitter[ min: %.3fs max: %.3fs ]" % (
min_delta, max_delta)
# std dev
self.last_printed_count = self.message_count
if self.SHOW_STD_DEV or self.show_only_rate:
std_dev = math.sqrt(sum((x - mean)**2 for x in self.times) / n)
message_std_dev = "std dev: %.5fs" % (std_dev)
if self.SHOW_WINDOW_SIZE or self.show_only_rate:
message_window = "window: %s" % (n + 1)
if self._scrapped_msgs > 0:
message_scrapped += "scrapped msgs: %s" % self._scrapped_msgs
self._state_message = ''
self._state_size_message = message_bytes
for msg in [
message_rate, message_jitter, message_std_dev,
message_window, message_scrapped
]:
if msg:
if self._state_message:
self._state_message += ' '
self._state_message += msg
self._print_status()
if self.show_only_rate:
self.display.append("%s %s" %
(self._state_message, message_bytes))
def _normilize_size_print(self, size):
if size > 999999:
return "%.2fMiB" % (size / 1048576.0)
if size > 999:
return "%.2fKiB" % (size / 1024.0)
return "%dB" % size
def _print_status(self):
text = '%s messages %s' % (self.message_count, self._state_message)
if self._latched:
text = "[latched] %s" % text
self.statusLabel.setText(text)
self.statusSizeLabel.setText(self._state_size_message)
def _append_text(self, text):
'''
Append echo text received through the SSH.
'''
with self.lock:
self._current_msg += text
if self._current_msg.find('---') != -1:
messages = self._current_msg.split('---')
for m in messages[:-1]:
current_time = time.time()
self._count_messages(current_time)
# limit the displayed text width
m = self._trim_width(m)
txt = '<pre style="background-color:#FFFCCC; color:#000000;font-family:Fixedsys,Courier; padding:10px;">---------- %s --------------------\n%s</pre>' % (
datetime.now().strftime("%d.%m.%Y %H:%M:%S.%f"), m)
# set the count of the displayed messages on receiving the first message
self._update_max_msg_count(txt)
self.display.append(txt)
self._current_msg = messages[-1]
self._print_status()
def _append_error_text(self, text):
'''
Append error text received through the SSH.
'''
with self.lock:
self._current_errmsg += text
if self._current_errmsg.find('\n') != -1:
messages = self._current_errmsg.split('\n')
for m in messages[:-1]:
txt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">%s</pre>' % m
self.display.append(txt)
self._current_errmsg = messages[-1]
def _append_text_hz(self, text):
'''
Append text received through the SSH for hz view.
'''
with self.lock:
self._current_msg += text
if self._current_msg.find('\n') != -1:
messages = self._current_msg.split('\n')
for m in messages[:-1]:
txt = '<div style="font-family:Fixedsys,Courier;">%s</div>' % (
m)
self.display.append(txt)
self._current_msg = messages[-1]
def _on_display_anchorClicked(self, url, user=None, pw=None):
try:
ok = False
if self.show_only_rate:
self.ssh_input_file, self.ssh_output_file, self.ssh_error_file, ok = nm.ssh(
).ssh_exec(url.host(), ['rostopic hz %s' % (self.topic)],
user,
pw,
auto_pw_request=True,
get_pty=True)
self.statusLabel.setText('connected to %s over SSH' %
url.host())
else:
nostr = '--nostr' if not self.showStringsCheckBox.isChecked(
) else ''
noarr = '--noarr' if not self.showArraysCheckBox.isChecked(
) else ''
self.ssh_input_file, self.ssh_output_file, self.ssh_error_file, ok = nm.ssh(
).ssh_exec(
url.host(),
['rostopic echo %s %s %s' % (nostr, noarr, self.topic)],
user,
pw,
auto_pw_request=True,
get_pty=True)
if ok:
self.display.clear()
target = self._read_output_hz if self.show_only_rate else self._read_output
thread = threading.Thread(target=target,
args=((self.ssh_output_file, )))
thread.setDaemon(True)
thread.start()
thread = threading.Thread(target=self._read_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 Exception as e:
self._append_error_text('%s\n' % e)
def _read_output_hz(self, output_file):
try:
while not output_file.closed:
text = output_file.read(1)
if text:
self.text_hz_signal.emit(text)
except Exception:
pass
def _read_output(self, output_file):
while not output_file.closed:
text = output_file.read(1)
if text:
self.text_signal.emit(text)
def _read_error(self, error_file):
try:
while not error_file.closed:
text = error_file.read(1)
if text:
self.text_error_signal.emit(text)
except Exception:
pass
# #############################################################################
# PARTS OF genpy/messages.py
# #############################################################################
@classmethod
def strify_message(cls,
val,
indent='',
time_offset=None,
current_time=None,
field_filter=None,
fixed_numeric_width=None,
digits_after_in_array=None):
"""
Convert value to string representation
:param val: to convert to string representation. Most likely a Message. ``Value``
:param indent: indentation. If indent is set, then the return value will have a leading \n, ``str``
:param time_offset: if not None, time fields will be displayed
as deltas from time_offset, ``Time``
:param current_time: currently not used. Only provided for API
compatibility. current_time passes in the current time with
respect to the message, ``Time``
:param field_filter: filter the fields that are strified for Messages, ``fn(Message)->iter(str)``
:returns: string (YAML) representation of message, ``str``
"""
type_ = type(val)
if type_ in (int, long, float) and fixed_numeric_width is not None:
if type_ is float:
return ('%.' + str(fixed_numeric_width) + 'f') % val
else:
return ('%d') % val
elif type_ in (int, long, float, bool):
return utf8(val)
elif isstring(val):
# TODO: need to escape strings correctly
if not val:
return "''"
return val
elif isinstance(val, TVal):
if time_offset is not None and isinstance(val, Time):
val = val - time_offset
if fixed_numeric_width is not None:
format_str = '%d'
sec_str = '\n%ssecs: ' % indent + (format_str % val.secs)
nsec_str = '\n%snsecs: ' % indent + (format_str % val.nsecs)
return sec_str + nsec_str
else:
return '\n%ssecs: %s\n%snsecs: %9d' % (indent, val.secs,
indent, val.nsecs)
elif type_ in (list, tuple):
if len(val) == 0:
return "[]"
val0 = val[0]
if type(val0) in (int,
float) and digits_after_in_array is not None:
list_str = '[' + ''.join(
cls.strify_message(v, indent, time_offset, current_time,
field_filter, digits_after_in_array) +
', ' for v in val).rstrip(', ') + ']'
return list_str
elif type(val0) in (int, float, str, bool):
# TODO: escape strings properly
return utf8(list(val))
else:
pref = indent + '- '
indent = indent + ' '
return '\n' + '\n'.join([
pref +
cls.strify_message(v, indent, time_offset, current_time,
field_filter, digits_after_in_array)
for v in val
])
elif isinstance(val, message.Message):
# allow caller to select which fields of message are strified
if field_filter is not None:
fields = list(field_filter(val))
else:
fields = val.__slots__
p = '%s%%s: %%s' % (indent)
ni = ' ' + indent
python_zip = None
if sys.hexversion > 0x03000000: # Python3
python_zip = zip
else: # Python2
python_zip = itertools.izip
slots = []
for f, t in python_zip(val.__slots__, val._slot_types):
if f in fields:
cval = _convert_getattr(val, f, t)
slot_name = f
if isinstance(cval, (list, tuple)):
slot_name = "%s[%d]" % (f, len(cval))
slots.append(p %
(utf8(slot_name),
cls.strify_message(
cval, ni, time_offset, current_time,
field_filter, fixed_numeric_width)))
vals = '\n'.join(slots)
if indent:
return '\n' + vals
else:
return vals
else:
return utf8(val) # punt
class LifeFrame(QFrame):
STATE_STOPPED = "stopped"
STATE_RUN = "running"
STATE_IDLE = "idle"
def __init__(self, parent=None):
super(LifeFrame, self).__init__(parent)
self._ui = Ui_life_frame()
self._motion = Rotate('/mobile_base/commands/velocity')
self._motion_thread = None
self._timer = QTimer()
#self._timer.setInterval(60000) #60s
self._timer.setInterval(250) #60s
QObject.connect(self._timer, SIGNAL('timeout()'), self, SLOT('update_progress_callback()'))
self._state = LifeFrame.STATE_STOPPED
self._is_alive = False # Used to indicate whether the frame is alive or not (see hibernate/restore methods)
def setupUi(self):
self._ui.setupUi(self)
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
self._motion.init(self._ui.angular_speed_spinbox.value())
def shutdown(self):
'''
Used to terminate the plugin
'''
rospy.loginfo("Kobuki TestSuite: life frame shutdown")
self._motion.shutdown()
##########################################################################
# Widget Management
##########################################################################
def hibernate(self):
'''
This gets called when the frame goes out of focus (tab switch).
Disable everything to avoid running N tabs in parallel when in
reality we are only running one.
'''
pass
def restore(self):
'''
Restore the frame after a hibernate.
'''
pass
##########################################################################
# Motion Callbacks
##########################################################################
def start(self):
self._state = LifeFrame.STATE_RUN
self._ui.run_progress.reset()
self._ui.idle_progress.reset()
self._motion_thread = WorkerThread(self._motion.execute, None)
self._motion_thread.start()
def stop(self):
self._state = LifeFrame.STATE_STOPPED
self._motion.stop()
if self._motion_thread:
self._motion_thread.wait()
##########################################################################
# Qt Callbacks
##########################################################################
@Slot()
def on_start_button_clicked(self):
self._ui.start_button.setEnabled(False)
self._ui.stop_button.setEnabled(True)
self._timer.start()
self.start()
@Slot()
def on_stop_button_clicked(self):
self.stop()
self._timer.stop()
self._ui.start_button.setEnabled(True)
self._ui.stop_button.setEnabled(False)
@pyqtSlot(float)
def on_angular_speed_spinbox_valueChanged(self, value):
self._motion.init(self._ui.angular_speed_spinbox.value())
##########################################################################
# Timer Callbacks
##########################################################################
@Slot()
def update_progress_callback(self):
if self._state == LifeFrame.STATE_RUN:
new_value = self._ui.run_progress.value()+1
if new_value == self._ui.run_progress.maximum():
print(" Switching to idle")
self._motion.stop()
self._state = LifeFrame.STATE_IDLE
else:
self._ui.run_progress.setValue(new_value)
if self._state == LifeFrame.STATE_IDLE:
new_value = self._ui.idle_progress.value()+1
if new_value == self._ui.idle_progress.maximum():
print(" Switching to run")
self.start()
else:
self._ui.idle_progress.setValue(new_value)
class AutopilotWidget(QWidget):
def __init__(self, parent):
# Init QWidget
super(AutopilotWidget, self).__init__(parent)
self.setObjectName('Autopilot Widget')
# set variables
self._quad_namespace = None
self._connected = False
self._arm_bridge_pub = None
self._start_pub = None
self._land_pub = None
self._off_pub = None
self._force_hover_pub = None
self._go_to_pose_pub = None
self._autopilot_feedback_sub = None
self._autopilot_feedback = quadrotor_msgs.AutopilotFeedback()
self._autopilot_feedback_stamp = rospy.Time.now()
# load UI
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'../../resource/autopilot_widget.ui')
loadUi(ui_file, self)
# Timer
self._update_info_timer = QTimer(self)
self._update_info_timer.timeout.connect(self.update_gui)
self._update_info_timer.start(100)
self.disconnect()
def connect(self, quad_namespace):
self._quad_namespace = quad_namespace
self._arm_bridge_pub = rospy.Publisher(quad_namespace + '/bridge/arm',
std_msgs.Bool,
queue_size=1)
self._start_pub = rospy.Publisher(quad_namespace + '/autopilot/start',
std_msgs.Empty,
queue_size=1)
self._land_pub = rospy.Publisher(quad_namespace + '/autopilot/land',
std_msgs.Empty,
queue_size=1)
self._off_pub = rospy.Publisher(quad_namespace + '/autopilot/off',
std_msgs.Empty,
queue_size=1)
self._force_hover_pub = rospy.Publisher(quad_namespace +
'/autopilot/force_hover',
std_msgs.Empty,
queue_size=1)
self._go_to_pose_pub = rospy.Publisher(quad_namespace +
'/autopilot/pose_command',
geometry_msgs.PoseStamped,
queue_size=1)
self._autopilot_feedback_sub = rospy.Subscriber(
quad_namespace + '/autopilot/feedback',
quadrotor_msgs.AutopilotFeedback, self.autopilot_feedback_cb)
self.button_arm_bridge.setEnabled(True)
self.button_start.setEnabled(True)
self.button_land.setEnabled(True)
self.button_off.setEnabled(True)
self.button_force_hover.setEnabled(True)
self.button_go_to_pose.setEnabled(True)
self._connected = True
def disconnect_pub_sub(self, pub):
if pub is not None:
pub.unregister()
pub = None
def disconnect(self):
self.disconnect_pub_sub(self._autopilot_feedback_sub)
self.disconnect_pub_sub(self._arm_bridge_pub)
self.disconnect_pub_sub(self._start_pub)
self.disconnect_pub_sub(self._land_pub)
self.disconnect_pub_sub(self._off_pub)
self.disconnect_pub_sub(self._force_hover_pub)
self.disconnect_pub_sub(self._go_to_pose_pub)
self.button_arm_bridge.setEnabled(False)
self.button_start.setEnabled(False)
self.button_land.setEnabled(False)
self.button_off.setEnabled(False)
self.button_force_hover.setEnabled(False)
self.button_go_to_pose.setEnabled(False)
self._connected = False
def autopilot_feedback_cb(self, msg):
self._autopilot_feedback = msg
self._autopilot_feedback_stamp = rospy.Time.now()
def update_gui(self):
if (self._connected and self.autopilot_feedback_available()):
# Autopilot status
self.autopilot_state.setText(
self.get_autopilot_state_name(
self._autopilot_feedback.autopilot_state))
self.control_command_delay.setText(
'%.1f' %
(self._autopilot_feedback.control_command_delay.to_sec() *
1000.0))
self.control_computation_time.setText(
'%.1f' %
(self._autopilot_feedback.control_computation_time.to_sec() *
1000.0))
self.trajectory_execution_left_duration.setText(
'%.1f' % self._autopilot_feedback.
trajectory_execution_left_duration.to_sec())
self.trajectories_left_in_queue.setText(
str(self._autopilot_feedback.trajectories_left_in_queue))
# Low-Level status
if (self._autopilot_feedback.low_level_feedback.battery_state ==
self._autopilot_feedback.low_level_feedback.BAT_GOOD):
self.status_battery_voltage.setStyleSheet(
'QLabel { color : green; }')
self.status_battery_state.setStyleSheet(
'QLabel { color : green; }')
elif (self._autopilot_feedback.low_level_feedback.battery_state ==
self._autopilot_feedback.low_level_feedback.BAT_LOW):
self.status_battery_voltage.setStyleSheet(
'QLabel { color : orange; }')
self.status_battery_state.setStyleSheet(
'QLabel { color : orange; }')
else:
self.status_battery_voltage.setStyleSheet(
'QLabel { color : red; }')
self.status_battery_state.setStyleSheet(
'QLabel { color : red; }')
self.status_battery_state.setText(
self.get_battery_state_name(
self._autopilot_feedback.low_level_feedback.battery_state))
self.status_battery_voltage.setText(
'%.2f' %
self._autopilot_feedback.low_level_feedback.battery_voltage)
self.status_control_mode.setText(
self.get_control_mode_name(
self._autopilot_feedback.low_level_feedback.control_mode))
# State Estimate
self.state_est_frame_id.setText(
'Frame ID: %s' %
self._autopilot_feedback.state_estimate.header.frame_id)
euler_angles = np.rad2deg(
self.quat_to_euler_angles(
self._autopilot_feedback.state_estimate.pose.pose.
orientation))
self.state_est_position.setText(
'x:%+.2f y:%+.2f z:%+.2f' %
(self._autopilot_feedback.state_estimate.pose.pose.position.x,
self._autopilot_feedback.state_estimate.pose.pose.position.y,
self._autopilot_feedback.state_estimate.pose.pose.position.z))
self.state_est_velocity.setText(
'x:%+.2f y:%+.2f z:%+.2f' %
(self._autopilot_feedback.state_estimate.twist.twist.linear.x,
self._autopilot_feedback.state_estimate.twist.twist.linear.y,
self._autopilot_feedback.state_estimate.twist.twist.linear.z))
self.state_est_orientation.setText(
'r:%+.1f p:%+.1f h:%+.1f' %
(euler_angles[0], euler_angles[1], euler_angles[2]))
self.state_est_body_rates.setText('x:%+.2f y:%+.2f z:%+.2f' % (
self._autopilot_feedback.state_estimate.twist.twist.angular.x /
math.pi * 180.0,
self._autopilot_feedback.state_estimate.twist.twist.angular.y /
math.pi * 180.0,
self._autopilot_feedback.state_estimate.twist.twist.angular.z /
math.pi * 180.0))
# Reference State
self.ref_position.setText(
'x:%(x)+.2f y:%(y)+.2f z:%(z)+.2f' % {
'x':
self._autopilot_feedback.reference_state.pose.position.x,
'y':
self._autopilot_feedback.reference_state.pose.position.y,
'z':
self._autopilot_feedback.reference_state.pose.position.z
})
self.ref_velocity.setText(
'x:%(x)+.2f y:%(y)+.2f z:%(z)+.2f' % {
'x':
self._autopilot_feedback.reference_state.velocity.linear.x,
'y':
self._autopilot_feedback.reference_state.velocity.linear.y,
'z':
self._autopilot_feedback.reference_state.velocity.linear.z
})
self.ref_acc.setText(
'x:%(x)+.2f y:%(y)+.2f z:%(z)+.2f' % {
'x':
self._autopilot_feedback.reference_state.acceleration.
linear.x,
'y':
self._autopilot_feedback.reference_state.acceleration.
linear.y,
'z':
self._autopilot_feedback.reference_state.acceleration.
linear.z
})
self.ref_heading.setText(
'%+.1f' % (self._autopilot_feedback.reference_state.heading /
math.pi * 180.0))
else:
# Autopilot status
self.autopilot_state.setText('Not Available')
self.control_command_delay.setText('0.0')
self.control_computation_time.setText('0.0')
self.trajectory_execution_left_duration.setText('0.0')
self.trajectories_left_in_queue.setText('0')
# Low-Level status
self.status_battery_voltage.setStyleSheet(
'QLabel { color : gray; }')
self.status_battery_state.setStyleSheet('QLabel { color : gray; }')
self.status_battery_state.setText('Not Available')
self.status_battery_voltage.setText('Not Available')
self.status_control_mode.setText('Not Available')
# State Estimate
self.state_est_frame_id.setText('Frame ID:')
self.state_est_position.setText('Not Available')
self.state_est_velocity.setText('Not Available')
self.state_est_orientation.setText('Not Available')
self.state_est_body_rates.setText('Not Available')
# Reference State
self.ref_position.setText('Not Available')
self.ref_velocity.setText('Not Available')
self.ref_acc.setText('Not Available')
self.ref_heading.setText('Not Available')
@Slot(bool)
def on_button_connect_clicked(self):
if (self._connected):
self.disconnect()
self.button_connect.setText('Connect')
else:
quad_namespace = self.namespace_text.text()
self.connect(quad_namespace)
self.button_connect.setText('Disconnect')
@Slot(bool)
def on_button_arm_bridge_clicked(self):
arm_message = std_msgs.Bool(True)
self._arm_bridge_pub.publish(arm_message)
@Slot(bool)
def on_button_start_clicked(self):
start_message = std_msgs.Empty()
self._start_pub.publish(start_message)
@Slot(bool)
def on_button_land_clicked(self):
land_message = std_msgs.Empty()
self._land_pub.publish(land_message)
@Slot(bool)
def on_button_off_clicked(self):
start_message = std_msgs.Empty()
self._off_pub.publish(start_message)
@Slot(bool)
def on_button_force_hover_clicked(self):
force_hover_msg = std_msgs.Empty()
self._force_hover_pub.publish(force_hover_msg)
@Slot(bool)
def on_button_go_to_pose_clicked(self):
try:
go_to_pose_msg = geometry_msgs.PoseStamped()
go_to_pose_msg.pose.position.x = float(self.go_to_pose_x.text())
go_to_pose_msg.pose.position.y = float(self.go_to_pose_y.text())
go_to_pose_msg.pose.position.z = float(self.go_to_pose_z.text())
heading = float(self.go_to_pose_heading.text()) / 180.0 * math.pi
go_to_pose_msg.pose.orientation.w = math.cos(heading / 2.0)
go_to_pose_msg.pose.orientation.z = math.sin(heading / 2.0)
self._go_to_pose_pub.publish(go_to_pose_msg)
except:
rospy.logwarn("Could not read and send go to pose message!")
def autopilot_feedback_available(self):
if (rospy.Time.now() - self._autopilot_feedback_stamp).to_sec() <= 1.0:
return True
return False
def get_autopilot_state_name(self, autopilot_state):
if (autopilot_state == self._autopilot_feedback.START):
return "START"
if (autopilot_state == self._autopilot_feedback.HOVER):
return "HOVER"
if (autopilot_state == self._autopilot_feedback.LAND):
return "LAND"
if (autopilot_state == self._autopilot_feedback.EMERGENCY_LAND):
return "EMERGENCY_LAND"
if (autopilot_state == self._autopilot_feedback.BREAKING):
return "BREAKING"
if (autopilot_state == self._autopilot_feedback.GO_TO_POSE):
return "GO_TO_POSE"
if (autopilot_state == self._autopilot_feedback.VELOCITY_CONTROL):
return "VELOCITY_CONTROL"
if (autopilot_state == self._autopilot_feedback.REFERENCE_CONTROL):
return "REFERENCE_CONTROL"
if (autopilot_state == self._autopilot_feedback.TRAJECTORY_CONTROL):
return "TRAJECTORY_CONTROL"
if (autopilot_state == self._autopilot_feedback.COMMAND_FEEDTHROUGH):
return "COMMAND_FEEDTHROUGH"
if (autopilot_state == self._autopilot_feedback.RC_MANUAL):
return "RC_MANUAL"
return "OFF"
def get_battery_state_name(self, battery_state):
if (battery_state ==
self._autopilot_feedback.low_level_feedback.BAT_GOOD):
return "Good"
if (battery_state ==
self._autopilot_feedback.low_level_feedback.BAT_LOW):
return "Low"
if (battery_state ==
self._autopilot_feedback.low_level_feedback.BAT_CRITICAL):
return "Critical"
return "Invalid"
def get_control_mode_name(self, control_mode):
if (control_mode ==
self._autopilot_feedback.low_level_feedback.ATTITUDE):
return "Attitude"
if (control_mode ==
self._autopilot_feedback.low_level_feedback.BODY_RATES):
return "Body Rates"
if (control_mode == self._autopilot_feedback.low_level_feedback.
ANGULAR_ACCELERATION):
return "Angular Accelerations"
if (control_mode ==
self._autopilot_feedback.low_level_feedback.ROTOR_THRUSTS):
return "Rotor Thrusts"
if (control_mode ==
self._autopilot_feedback.low_level_feedback.RC_MANUAL):
return "RC_Manual"
return "None"
def quat_to_euler_angles(self, q):
# Computes the euler angles from a unit quaternion using the
# z-y-x convention.
# euler_angles = [roll pitch yaw]'
# A quaternion is defined as q = [qw qx qy qz]'
# where qw is the real part.
euler_angles = np.zeros((3, 1))
euler_angles[0] = np.arctan2(
2 * q.w * q.x + 2 * q.y * q.z,
q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)
euler_angles[1] = -np.arcsin(2 * q.x * q.z - 2 * q.w * q.y)
euler_angles[2] = np.arctan2(
2 * q.w * q.z + 2 * q.x * q.y,
q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z)
return euler_angles
class BTWidget(QWidget):
_redraw_interval = 40
_deferred_fit_in_view = Signal()
def __init__(self):
super(BTWidget, self).__init__()
self.setObjectName('BTWidget')
self._graph = None
self._current_dotcode = None
self._initialized = False
# dot_to_qt transforms into Qt elements using dot layout
self.dot_to_qt = DotToQtGenerator()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_bt'), 'resource', 'rqt_bt.ui')
loadUi(ui_file, self,
{'InteractiveGraphicsView': InteractiveGraphicsView})
self.refresh_timer = QTimer(self)
self.refresh_timer.start(self._redraw_interval)
self.refresh_timer.timeout.connect(self._refresh_rosgraph)
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self.graphics_view.setScene(self._scene)
self.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh'))
self.refresh_graph_push_button.clicked.connect(self._update_rosgraph)
self.highlight_connections_check_box.toggled.connect(
self._redraw_graph_view)
self.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view)
self.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original'))
self.fit_in_view_push_button.clicked.connect(self._fit_in_view)
self.depth_spin_box.setMinimum(-1)
self.depth_spin_box.valueChanged.connect(self._refresh_rosgraph)
self.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as'))
self.save_dot_push_button.clicked.connect(self._save_dot)
self.save_as_svg_push_button.setIcon(
QIcon.fromTheme('document-save-as'))
self.save_as_svg_push_button.clicked.connect(self._save_svg)
self.save_as_image_push_button.setIcon(QIcon.fromTheme('image'))
self.save_as_image_push_button.clicked.connect(self._save_image)
self.run_push_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.run_push_button.clicked.connect(self._run_bt)
self._update_rosgraph()
self._deferred_fit_in_view.connect(self._fit_in_view,
Qt.QueuedConnection)
self._deferred_fit_in_view.emit()
# generator builds tree graph
bt_sub_name = '/cyborg/bt/behavior_tree'
bt_update_sub_name = '/cyborg/bt/behavior_tree_updates'
bt_enabled_sub_name = '/cyborg/bt/enabled'
bt_enable_srv_name = '/cyborg/bt/enable'
self._bt_enabled = True
rospy.Subscriber(bt_enabled_sub_name, Bool, self._bt_enabled_cb)
self._bt_enable_srv = rospy.ServiceProxy(bt_enable_srv_name, SetBool)
bt_data = BTData(bt_sub_name, bt_update_sub_name)
self.dotcode_generator = RosBTDotcodeGenerator(bt_data)
def _bt_enabled_cb(self, msg):
self._bt_enabled = msg.data
if self._bt_enabled:
self.run_push_button.setIcon(
QIcon.fromTheme('media-playback-pause'))
else:
self.run_push_button.setIcon(
QIcon.fromTheme('media-playback-start'))
def _run_bt(self):
enable_req = False if self._bt_enabled else True
if self._bt_enable_srv(data=enable_req).success:
self._bt_enabled = enable_req
def _update_rosgraph(self):
self._graph = rosgraph.impl.graph.Graph()
self._graph.set_master_stale(5.0)
self._graph.set_node_stale(5.0)
self._graph.update()
self._refresh_rosgraph()
def _refresh_rosgraph(self):
if not self._initialized:
return
self._update_graph_view(self._generate_dotcode())
def _generate_dotcode(self):
depth = self.depth_spin_box.value()
return self.dotcode_generator.generate_dotcode(max_depth=depth)
def _update_graph_view(self, dotcode):
if dotcode == self._current_dotcode:
return
self._current_dotcode = dotcode
self._redraw_graph_view()
def _generate_tool_tip(self, url):
if url is not None and ':' in url:
item_type, item_path = url.split(':', 1)
if item_type == 'node':
tool_tip = 'Node:\n %s' % (item_path)
service_names = rosservice.get_service_list(node=item_path)
if service_names:
tool_tip += '\nServices:'
for service_name in service_names:
try:
service_type = rosservice.get_service_type(
service_name)
tool_tip += '\n %s [%s]' % (service_name,
service_type)
except rosservice.ROSServiceIOException as e:
tool_tip += '\n %s' % (e)
return tool_tip
elif item_type == 'topic':
topic_type, topic_name, _ = rostopic.get_topic_type(item_path)
return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type)
return url
def _redraw_graph_view(self):
self._scene.clear()
if self.highlight_connections_check_box.isChecked():
highlight_level = 3
else:
highlight_level = 1
# layout graph and create qt items
(nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(
self._current_dotcode,
highlight_level=highlight_level,
same_label_siblings=True)
for node_item in nodes.values():
self._scene.addItem(node_item)
for edge_items in edges.values():
for edge_item in edge_items:
edge_item.add_to_scene(self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
if self.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
def _fit_in_view(self):
self.graphics_view.fitInView(self._scene.itemsBoundingRect(),
Qt.KeepAspectRatio)
def _save_dot(self):
file_name, _ = QFileDialog.getSaveFileName(
self, self.tr('Save as DOT'), 'rosgraph.dot',
self.tr('DOT graph (*.dot)'))
if file_name is None or file_name == '':
return
handle = QFile(file_name)
if not handle.open(QIODevice.WriteOnly | QIODevice.Text):
return
handle.write(self._current_dotcode)
handle.close()
def _save_svg(self):
file_name, _ = QFileDialog.getSaveFileName(
self, self.tr('Save as SVG'), 'rosgraph.svg',
self.tr('Scalable Vector Graphic (*.svg)'))
if file_name is None or file_name == '':
return
generator = QSvgGenerator()
generator.setFileName(file_name)
generator.setSize((self._scene.sceneRect().size() * 2.0).toSize())
painter = QPainter(generator)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
def _save_image(self):
file_name, _ = QFileDialog.getSaveFileName(
self, self.tr('Save as image'), 'rosgraph.png',
self.tr('Image (*.bmp *.jpg *.png *.tiff)'))
if file_name is None or file_name == '':
return
img = QImage((self._scene.sceneRect().size() * 2.0).toSize(),
QImage.Format_ARGB32_Premultiplied)
painter = QPainter(img)
painter.setRenderHint(QPainter.Antialiasing)
self._scene.render(painter)
painter.end()
img.save(file_name)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('auto_fit_graph_check_box_state',
self.auto_fit_graph_check_box.isChecked())
instance_settings.set_value(
'highlight_connections_check_box_state',
self.highlight_connections_check_box.isChecked())
instance_settings.set_value('depth_spin_box_value',
self.depth_spin_box.value())
def restore_settings(self, plugin_settings, instance_settings):
self.auto_fit_graph_check_box.setChecked(
instance_settings.value('auto_fit_graph_check_box_state', True) in
[True, 'true'])
self.highlight_connections_check_box.setChecked(
instance_settings.value('highlight_connections_check_box_state',
True) in [True, 'true'])
self.depth_spin_box.setValue(
int(instance_settings.value('depth_spin_box_value', -1)))
self._initialized = True
self._refresh_rosgraph()
class CalibWidget(QWidget):
_sub_pattern_detections = None
_sub_calibration_output = None
_num_pattern_detections = 0
_calibration_output = ""
_update_required = True
def __init__(self):
print('constructor')
# Init QWidget
super(CalibWidget, self).__init__()
self.setObjectName('CalibWidget')
# load UI
ui_file = os.path.join(rospkg.RosPack().get_path('dvs_calibration_gui'), 'resource', 'widget.ui')
loadUi(ui_file, self)
# init and start update timer for data, the timer calls the function update_info all 40ms
self._update_info_timer = QTimer(self)
self._update_info_timer.timeout.connect(self.update_info)
self._update_info_timer.start(40)
self.button_reset.pressed.connect(self.on_button_reset_pressed)
self.button_start.pressed.connect(self.on_button_start_calibration_pressed)
self.button_save.pressed.connect(self.on_button_save_calibration_pressed)
self._sub_pattern_detections = rospy.Subscriber('dvs_calibration/pattern_detections', Int32, self.pattern_detections_cb)
self._sub_calibration_output = rospy.Subscriber('dvs_calibration/output', String, self.calibration_output_cb)
print('reset')
self.on_button_reset_pressed()
print('reset done')
def unregister(self):
print('Nothing to do')
def pattern_detections_cb(self, msg):
self._num_pattern_detections = msg.data
if (self._num_pattern_detections > 0):
self.button_start.setEnabled( True )
self._update_required = True
def calibration_output_cb(self, msg):
self._calibration_output = msg.data
self._update_required = True
def update_info(self):
if (self._update_required):
self.numDetectionsLabel.setText(str(self._num_pattern_detections))
self.calibrationResultText.setPlainText(self._calibration_output)
self._update_required = False
@Slot(bool)
def on_button_reset_pressed(self):
self._num_pattern_detections = 0
self._calibration_output = ""
self._update_required = True
self.button_start.setEnabled( False )
try:
rospy.wait_for_service('dvs_calibration/reset', 1)
try:
reset_service = rospy.ServiceProxy('dvs_calibration/reset', Empty)
resp = reset_service()
except rospy.ServiceException, e:
print "Service call failed: %s"%e
except:
print "service not available..."
pass
@Slot(bool)
def on_button_start_calibration_pressed(self):
self.button_start.setEnabled( False )
try:
rospy.wait_for_service('dvs_calibration/start', 1)
try:
start_calibration_service = rospy.ServiceProxy('dvs_calibration/start', Empty)
resp = start_calibration_service()
except rospy.ServiceException, e:
print "Service call failed: %s"%e
except:
print "service not available..."
pass
@Slot(bool)
def on_button_save_calibration_pressed(self):
try:
rospy.wait_for_service('dvs_calibration/save', 1)
try:
save_calibration_service = rospy.ServiceProxy('dvs_calibration/save', Empty)
resp = save_calibration_service()
except rospy.ServiceException, e:
print "Service call failed: %s"%e
except:
print "service not available..."
pass
class ParamWidget(QWidget):
"""
main class inherits from the ui window class.
You can specify the topics that the topic pane.
ParamWidget.start must be called in order to update topic pane.
"""
SELECT_BY_NAME = 0
SELECT_BY_MSGTYPE = 1
# _column_names = ['topic', 'type', 'bandwidth', 'rate', 'value', 'checkbox']
_column_names = ['topic', 'type', 'min', 'value', 'max', 'checkbox']
selectionChanged = pyqtSignal(dict, name='selectionChanged')
def keyPressEvent(self, event):
event.ignore()
def keyReleaseEvent(self, event):
event.ignore()
def __init__(self,
plugin=None,
selected_topics=None,
select_topic_type=SELECT_BY_NAME):
"""
@type selected_topics: list of tuples.
@param selected_topics: [($NAME_TOPIC$, $TYPE_TOPIC$), ...]
@type select_topic_type: int
@param select_topic_type: Can specify either the name of topics or by
the type of topic, to filter the topics to
show. If 'select_topic_type' argument is
None, this arg shouldn't be meaningful.
"""
super(ParamWidget, self).__init__()
self._select_topic_type = select_topic_type
self.setFocusPolicy(Qt.StrongFocus)
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_dyn_tune'), 'resource',
'ParamWidget.ui')
loadUi(ui_file, self)
self._plugin = plugin
self.topics_tree_widget.sortByColumn(0, Qt.AscendingOrder)
header = self.topics_tree_widget.header()
try:
setSectionResizeMode = header.setSectionResizeMode # Qt5
except AttributeError:
setSectionResizeMode = header.setResizeMode # Qt4
setSectionResizeMode(QHeaderView.ResizeToContents)
header.customContextMenuRequested.connect(
self.handle_header_view_customContextMenuRequested)
header.setContextMenuPolicy(Qt.CustomContextMenu)
# for i in range(len(self._column_names)):
# setSectionResizeMode(i, QHeaderView.Stretch)
header.setStretchLastSection(False)
setSectionResizeMode(0, QHeaderView.Stretch)
setSectionResizeMode(self._column_names.index("value"),
QHeaderView.Stretch)
setSectionResizeMode(self._column_names.index("checkbox"),
QHeaderView.Fixed)
# Whether to get all topics or only the topics that are set in advance.
# Can be also set by the setter method "set_selected_topics".
self._selected_topics = selected_topics
self._selected_items = []
self._current_topic_list = []
self._topics = {}
self._tree_items = {}
self._column_index = {}
for column_name in self._column_names:
self._column_index[column_name] = len(self._column_index)
self.topics_tree_widget.itemExpanded.connect(self.expanded)
self.topics_tree_widget.itemCollapsed.connect(self.collapsed)
self.topics_tree_widget.itemChanged.connect(self.itemChanged)
self.topics_tree_widget.setAlternatingRowColors(True)
delegate = EditorDelegate()
self.topics_tree_widget.setItemDelegate(delegate)
self.topics_tree_widget.setEditTriggers(
QAbstractItemView.AnyKeyPressed | QAbstractItemView.DoubleClicked)
hitem = self.topics_tree_widget.headerItem()
hitem.setTextAlignment(self._column_names.index("min"),
Qt.AlignRight | Qt.AlignVCenter)
hitem.setTextAlignment(self._column_names.index("max"),
Qt.AlignLeft | Qt.AlignVCenter)
hitem.setTextAlignment(self._column_names.index("value"),
Qt.AlignHCenter | Qt.AlignVCenter)
hitem.setTextAlignment(self._column_names.index("type"),
Qt.AlignHCenter | Qt.AlignVCenter)
# init and start update timer
self._timer_refresh_topics = QTimer(self)
self._timer_refresh_topics.timeout.connect(self.refresh_topics)
@Slot(dict)
def selection_changed(data):
print "#\n#\n#\nthe selcted items are:", data, "\n\n"
self.selectionChanged.connect(selection_changed)
def set_topic_specifier(self, specifier):
self._select_topic_type = specifier
def start(self):
"""
This method needs to be called to start updating topic pane.
"""
self._timer_refresh_topics.start(1000)
@Slot('QTreeWidgetItem', int)
def itemChanged(self, item, column):
# print "<<<<<<<<<<<<<<<<<<<<<<<<<< item changed >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
selected = self.get_selected()
if item._topic_name in selected and self._column_names[column] in [
"min", "max", "value"
]:
self.selectionChanged.emit(selected)
@Slot('QTreeWidgetItem')
def expanded(self, item):
name = item.data(0, Qt.UserRole)
if not isinstance(item, TreeWidgetItem) or not item._is_topic:
return
print "expanded", name
self._topics[item._topic_name]['info'].start_monitoring()
@Slot('QTreeWidgetItem')
def collapsed(self, item):
name = item.data(0, Qt.UserRole)
if not isinstance(item, TreeWidgetItem) or not item._is_topic:
return
print "collapsed", name
self._topics[item._topic_name]['info'].stop_monitoring()
_items_param = {}
def get_desc(self, item):
desc = {}
if item._topic_name in self._current_params:
desc = self._current_params[item._topic_name]
vmin = item.data(self._column_names.index("min"), Qt.EditRole)
vmax = item.data(self._column_names.index("max"), Qt.EditRole)
val = item.data(self._column_names.index("value"), Qt.EditRole)
def tryFloat(s):
try:
return float(s)
except:
return None
vmin = tryFloat(vmin)
vmax = tryFloat(vmax)
val = tryFloat(val)
desc["min"] = vmin
desc["max"] = vmax
desc["default"] = val
return desc
def get_selected(self):
# param_name:desc
return {
param: self.get_desc(self._tree_items[param])
for param in self._selected_items if param in self._current_params
}
pass
def insert_param(self, param_name, param_desc, parent=None):
if parent == None:
parent = self.topics_tree_widget
pnames = param_name.split("/")
ns = ""
item = parent
for name in pnames:
if name == "":
continue
_name = "/" + name
ns = ns + _name
if ns not in self._tree_items:
_item = TreeWidgetItem(self._toggle_monitoring, ns, item)
_item.setText(self._column_index['topic'], _name)
_item.setData(0, Qt.UserRole, "name_space")
_item.setFlags(Qt.ItemIsEnabled | Qt.ItemIsUserCheckable
| Qt.ItemIsTristate)
self._tree_items[ns] = _item
item = self._tree_items[ns]
item.setFlags(Qt.ItemIsEditable | Qt.ItemIsEnabled
| Qt.ItemIsSelectable | Qt.ItemIsUserCheckable)
item.setData(self._column_index['min'], Qt.EditRole,
str(param_desc["min"]))
item.setData(self._column_index['max'], Qt.EditRole,
str(param_desc["max"]))
item.setData(self._column_index['value'], Qt.EditRole,
str(param_desc["default"]))
item.setData(self._column_index['type'], Qt.EditRole,
str(param_desc["type"]))
self._items_param[item] = param_name
print param_name, " added"
pass
_current_params = {}
# TODO: implement the delete mechanism
def delete_param(self, param_name, parent=None):
pass
@Slot()
def refresh_topics(self):
"""
refresh tree view items
"""
nodes = dynamic_reconfigure.find_reconfigure_services()
# dparams = []
dparams = {}
for node in nodes:
client = dynamic_reconfigure.client.Client(node, timeout=3)
gdesc = client.get_group_descriptions()
for pdesc in gdesc["parameters"]:
param = node + "/" + pdesc["name"]
dparams[param] = pdesc
for param, desc in self._current_params.items():
if param not in dparams:
del self._current_params[param]
# TODO: delete the tree widget item
for param, desc in dparams.items():
if param not in self._current_params:
self._current_params[param] = desc
self.insert_param(param, desc)
def _update_topics_data(self):
selected_dict = {
self._selected_items[index]: index
for index in range(len(self._selected_items))
}
print selected_dict
for topic in self._topics.values():
topic_info = topic['info']
if topic_info.monitoring:
# update rate
rate, _, _, _ = topic_info.get_hz()
rate_text = '%1.2f' % rate if rate != None else 'unknown'
# update bandwidth
bytes_per_s, _, _, _ = topic_info.get_bw()
if bytes_per_s is None:
bandwidth_text = 'unknown'
elif bytes_per_s < 1000:
bandwidth_text = '%.2fB/s' % bytes_per_s
elif bytes_per_s < 1000000:
bandwidth_text = '%.2fKB/s' % (bytes_per_s / 1000.)
else:
bandwidth_text = '%.2fMB/s' % (bytes_per_s / 1000000.)
# update values
value_text = ''
self.update_value(topic_info._topic_name,
topic_info.last_message)
else:
rate_text = ''
bandwidth_text = ''
value_text = 'not monitored' if topic_info.error is None else topic_info.error
self._tree_items[topic_info._topic_name].setText(
self._column_index['value'], value_text)
def update_value(self, topic_name, message):
if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
for slot_name in message.__slots__:
self.update_value(topic_name + '/' + slot_name,
getattr(message, slot_name))
elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(
message[0], '__slots__'):
for index, slot in enumerate(message):
if topic_name + '[%d]' % index in self._tree_items:
self.update_value(topic_name + '[%d]' % index, slot)
else:
base_type_str, _ = self._extract_array_info(
self._tree_items[topic_name].text(
self._column_index['type']))
self._recursive_create_widget_items(
self._tree_items[topic_name],
topic_name + '[%d]' % index, base_type_str, slot)
# remove obsolete children
if len(message) < self._tree_items[topic_name].childCount():
for i in range(len(message),
self._tree_items[topic_name].childCount()):
item_topic_name = topic_name + '[%d]' % i
self._recursive_delete_widget_items(
self._tree_items[item_topic_name])
else:
if topic_name in self._tree_items:
self._tree_items[topic_name].setText(
self._column_index['value'], repr(message))
def _extract_array_info(self, type_str):
array_size = None
if '[' in type_str and type_str[-1] == ']':
type_str, array_size_str = type_str.split('[', 1)
array_size_str = array_size_str[:-1]
if len(array_size_str) > 0:
array_size = int(array_size_str)
else:
array_size = 0
return type_str, array_size
def _recursive_create_widget_items(self, parent, topic_name, type_name,
message):
if parent is self.topics_tree_widget:
# show full topic name with preceding namespace on toplevel item
topic_text = topic_name
_item = parent
topic_names = topic_name.split('/')
name_space = ""
for name in topic_names:
if name == "":
continue
_name = "/" + name
name_space = name_space + _name
if name_space not in self._tree_items:
is_topic = False
if name_space == topic_name:
is_topic = True
_item = TreeWidgetItem(self._toggle_monitoring,
name_space,
_item,
is_topic=is_topic)
_item.setText(self._column_index['topic'], _name)
_item.setText(self._column_index['type'], type_name)
_item.setData(0, Qt.UserRole, name_space)
self._tree_items[name_space] = _item
_item = self._tree_items[name_space]
item = _item
else:
topic_text = topic_name.split('/')[-1]
if '[' in topic_text:
topic_text = topic_text[topic_text.index('['):]
item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent)
item.setText(self._column_index['topic'], topic_text)
item.setText(self._column_index['type'], type_name)
item.setData(0, Qt.UserRole, topic_name)
self._tree_items[topic_name] = item
if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
for slot_name, type_name in zip(message.__slots__,
message._slot_types):
self._recursive_create_widget_items(
item, topic_name + '/' + slot_name, type_name,
getattr(message, slot_name))
else:
base_type_str, array_size = self._extract_array_info(type_name)
try:
base_instance = roslib.message.get_message_class(
base_type_str)()
except (ValueError, TypeError):
base_instance = None
if array_size is not None and hasattr(base_instance, '__slots__'):
for index in range(array_size):
self._recursive_create_widget_items(
item, topic_name + '[%d]' % index, base_type_str,
base_instance)
return item
def _toggle_monitoring(self, topic_name):
item = self._tree_items[topic_name]
if item.checkState(CHECK_COLUMN):
print "start %s" % topic_name
if topic_name not in self._selected_items and topic_name in self._current_params:
self._selected_items.append(topic_name)
else:
print "stop %s" % topic_name
if topic_name in self._selected_items:
self._selected_items.remove(topic_name)
item.setText(CHECK_COLUMN, '')
self.selectionChanged.emit(self.get_selected())
def _recursive_delete_widget_items(self, item):
def _recursive_remove_items_from_tree(item):
for index in reversed(range(item.childCount())):
_recursive_remove_items_from_tree(item.child(index))
topic_name = item.data(0, Qt.UserRole)
del self._tree_items[topic_name]
_recursive_remove_items_from_tree(item)
item.parent().removeChild(item)
@Slot('QPoint')
def handle_header_view_customContextMenuRequested(self, pos):
header = self.topics_tree_widget.header()
# show context menu
menu = QMenu(self)
action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize')
action = menu.exec_(header.mapToGlobal(pos))
# evaluate user action
if action is action_toggle_auto_resize:
if header.resizeMode(0) == QHeaderView.ResizeToContents:
header.setResizeMode(QHeaderView.Interactive)
else:
header.setResizeMode(QHeaderView.ResizeToContents)
@Slot('QPoint')
def on_topics_tree_widget_customContextMenuRequested(self, pos):
item = self.topics_tree_widget.itemAt(pos)
if item is None:
return
# show context menu
menu = QMenu(self)
action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'),
'Expand All Children')
action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'),
'Collapse All Children')
action = menu.exec_(self.topics_tree_widget.mapToGlobal(pos))
# evaluate user action
if action in (action_item_expand, action_item_collapse):
expanded = (action is action_item_expand)
def recursive_set_expanded(item):
item.setExpanded(expanded)
for index in range(item.childCount()):
recursive_set_expanded(item.child(index))
recursive_set_expanded(item)
def shutdown_plugin(self):
for topic in self._topics.values():
topic['info'].stop_monitoring()
self._timer_refresh_topics.stop()
def set_selected_topics(self, selected_topics):
"""
@param selected_topics: list of tuple. [(topic_name, topic_type)]
@type selected_topics: []
"""
rospy.logdebug('set_selected_topics topics={}'.format(
len(selected_topics)))
self._selected_topics = selected_topics
# TODO(Enhancement) Save/Restore tree expansion state
def save_settings(self, plugin_settings, instance_settings):
header_state = self.topics_tree_widget.header().saveState()
instance_settings.set_value('tree_widget_header_state', header_state)
def restore_settings(self, pluggin_settings, instance_settings):
if instance_settings.contains('tree_widget_header_state'):
header_state = instance_settings.value('tree_widget_header_state')
if not self.topics_tree_widget.header().restoreState(header_state):
rospy.logwarn("rqt_dyn_tune: Failed to restore header state.")
class EnhancedLineEdit(QLineEdit):
stop_signal = Signal()
''' stop button was pressed '''
refresh_signal = Signal(str)
''' sends a refresh signal with current text. This signal is emmited if text was changed or button was pressed '''
def __init__(self, parent=None):
QLineEdit.__init__(self, parent=None)
self.process_active = False
# create a reload button with icon
self.button_reload = button_reload = QToolButton(self)
icon = QIcon.fromTheme("view-refresh",
QIcon(":/icons/oxygen_view_refresh.png"))
button_reload.setIcon(icon)
button_reload.setCursor(Qt.ArrowCursor)
button_reload.setStyleSheet(
"QToolButton { border: none; padding: 0px; }")
# create a stop button with icon
self.button_stop = button_stop = QToolButton(self)
icon = QIcon.fromTheme("process-stop",
QIcon(":/icons/oxygen_view_refresh.png"))
button_stop.setIcon(icon)
button_stop.setCursor(Qt.ArrowCursor)
button_stop.setStyleSheet(
"QToolButton { border: none; padding: 0px; }")
button_stop.hide()
# signals, clear lineEdit if btn pressed; change btn visibility on input
button_reload.clicked.connect(self._emit_refresh_text)
self.textChanged[str].connect(self.update_close_button)
button_stop.clicked.connect(self._process_stop)
frameWidth = self.style().pixelMetric(QStyle.PM_DefaultFrameWidth)
self.setStyleSheet("QLineEdit { padding-right: %dpx; } " %
(button_reload.sizeHint().width() + frameWidth + 1))
msz = self.minimumSizeHint()
self.setMinimumSize(
max(msz.width(),
button_reload.sizeHint().height() + frameWidth * 2 + 2),
max(msz.height(),
button_reload.sizeHint().height() + frameWidth * 2 + 2))
self._timer = QTimer(self)
self._timer.setSingleShot(True)
self._timer.setInterval(500)
self._timer.timeout.connect(self._emit_refresh_text)
def resizeEvent(self, event):
sz = self.button_reload.sizeHint()
frameWidth = self.style().pixelMetric(QStyle.PM_DefaultFrameWidth)
self.button_reload.move(self.rect().right() - frameWidth - sz.width(),
(self.rect().bottom() + 1 - sz.height()) / 2)
self.button_stop.move(self.rect().right() - frameWidth - sz.width(),
(self.rect().bottom() + 1 - sz.height()) / 2)
def update_close_button(self, text):
self._timer.stop()
self._timer.start()
# self.button_reload.setVisible(True if text else False)
def set_process_active(self, state):
if self.process_active != state:
self.process_active = state
self.button_reload.setVisible(not state)
self.button_stop.setVisible(state)
def _process_stop(self):
self.stop_signal.emit()
def _emit_refresh_text(self):
self.set_process_active(True)
self.refresh_signal.emit(self.text())
def keyPressEvent(self, event):
'''
Enable the ESC handling
'''
if event.key() == Qt.Key_Escape and self.text():
self.setText('')
self._timer.stop()
elif event.key() in [Qt.Key_Return, Qt.Key_Enter]:
self._timer.stop()
self._emit_refresh_text()
else:
event.accept()
QLineEdit.keyPressEvent(self, event)
class JointTrajectoryController(Plugin):
"""
Graphical frontend for a C{JointTrajectoryController}.
There are two modes for interacting with a controller:
1. B{Monitor mode} Joint displays are updated with the state reported
by the controller. This is a read-only mode and does I{not} send
control commands. Every time a new controller is selected, it starts
in monitor mode for safety reasons.
2. B{Control mode} Joint displays update the control command that is
sent to the controller. Commands are sent periodically evan if the
displays are not being updated by the user.
To control the aggressiveness of the motions, the maximum speed of the
sent commands can be scaled down using the C{Speed scaling control}
This plugin is able to detect and keep track of all active controller
managers, as well as the JointTrajectoryControllers that are I{running}
in each controller manager.
For a controller to be compatible with this plugin, it must comply with
the following requisites:
- The controller type contains the C{JointTrajectoryController}
substring, e.g., C{position_controllers/JointTrajectoryController}
- The controller exposes the C{command} and C{state} topics in its
ROS interface.
Additionally, there must be a URDF loaded with a valid joint limit
specification, namely position (when applicable) and velocity limits.
A reference implementation of the C{JointTrajectoryController} is
available in the C{joint_trajectory_controller} ROS package.
"""
_cmd_pub_freq = 10.0 # Hz
_widget_update_freq = 30.0 # Hz
_ctrlrs_update_freq = 1 # Hz
_min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration
jointStateChanged = Signal([dict])
def __init__(self, context):
super(JointTrajectoryController, self).__init__(context)
self.setObjectName('JointTrajectoryController')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'),
'resource',
'joint_trajectory_controller.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JointTrajectoryControllerUi')
# Setup speed scaler
speed_scaling = DoubleEditor(1.0, 100.0)
speed_scaling.spin_box.setSuffix('%')
speed_scaling.spin_box.setValue(50.0)
speed_scaling.spin_box.setDecimals(0)
speed_scaling.setEnabled(False)
self._widget.speed_scaling_layout.addWidget(speed_scaling)
self._speed_scaling_widget = speed_scaling
speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
self._on_speed_scaling_change(speed_scaling.value())
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._jtc_name = [] # Name of selected joint trajectory controller
self._cm_ns = [] # Namespace of the selected controller manager
self._joint_pos = {} # name->pos map for joints of selected controller
self._joint_names = [] # Ordered list of selected controller joints
self._robot_joint_limits = {} # Lazily evaluated on first use
# Timer for sending commands to active controller
self._update_cmd_timer = QTimer(self)
self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
self._update_cmd_timer.timeout.connect(self._update_cmd_cb)
# Timer for updating the joint widgets from the controller state
self._update_act_pos_timer = QTimer(self)
self._update_act_pos_timer.setInterval(1000.0 /
self._widget_update_freq)
self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)
# Timer for controller manager updates
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_jtc_list_timer = QTimer(self)
self._update_jtc_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()
# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber
self._list_controllers = None
def shutdown_plugin(self):
self._update_cmd_timer.stop()
self._update_act_pos_timer.stop()
self._update_cm_list_timer.stop()
self._update_jtc_list_timer.stop()
self._unregister_state_sub()
self._unregister_cmd_pub()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
instance_settings.set_value('jtc_name', self._jtc_name)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
# Resore last session's controller, if running
self._update_jtc_list()
jtc_name = instance_settings.value('jtc_name')
jtc_combo = self._widget.jtc_combo
jtc_list = [jtc_combo.itemText(i) for i in range(jtc_combo.count())]
try:
idx = jtc_list.index(jtc_name)
jtc_combo.setCurrentIndex(idx)
except (ValueError):
pass
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
update_combo(self._widget.cm_combo, self._list_cm())
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if not self._list_controllers:
self._widget.jtc_combo.clear()
return
# List of running controllers with a valid joint limits specification
# for _all_ their joints
running_jtc = self._running_jtc_info()
if running_jtc and not self._robot_joint_limits:
self._robot_joint_limits = get_joint_limits() # Lazy evaluation
valid_jtc = []
for jtc_info in running_jtc:
has_limits = all(name in self._robot_joint_limits
for name in _jtc_joint_names(jtc_info))
if has_limits:
valid_jtc.append(jtc_info);
valid_jtc_names = [data.name for data in valid_jtc]
# Update widget
update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))
def _on_speed_scaling_change(self, val):
self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
def _on_joint_state_change(self, actual_pos):
assert(len(actual_pos) == len(self._joint_pos))
for name in actual_pos.keys():
self._joint_pos[name]['position'] = actual_pos[name]
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
if cm_ns:
self._list_controllers = ControllerLister(cm_ns)
# NOTE: Clear below is important, as different controller managers
# might have controllers with the same name but different
# configurations. Clearing forces controller re-discovery
self._widget.jtc_combo.clear()
self._update_jtc_list()
else:
self._list_controllers = None
def _on_jtc_change(self, jtc_name):
self._unload_jtc()
self._jtc_name = jtc_name
if self._jtc_name:
self._load_jtc()
def _on_jtc_enabled(self, val):
# Don't allow enabling if there are no controllers selected
if not self._jtc_name:
self._widget.enable_button.setChecked(False)
return
# Enable/disable joint displays
for joint_widget in self._joint_widgets():
joint_widget.setEnabled(val)
# Enable/disable speed scaling
self._speed_scaling_widget.setEnabled(val)
if val:
# Widgets send desired position commands to controller
self._update_act_pos_timer.stop()
self._update_cmd_timer.start()
else:
# Controller updates widgets with actual position
self._update_cmd_timer.stop()
self._update_act_pos_timer.start()
def _load_jtc(self):
# Initialize joint data corresponding to selected controller
running_jtc = self._running_jtc_info()
self._joint_names = next(_jtc_joint_names(x) for x in running_jtc
if x.name == self._jtc_name)
for name in self._joint_names:
self._joint_pos[name] = {}
# Update joint display
try:
layout = self._widget.joint_group.layout()
for name in self._joint_names:
limits = self._robot_joint_limits[name]
joint_widget = DoubleEditor(limits['min_position'],
limits['max_position'])
layout.addRow(name, joint_widget)
# NOTE: Using partial instead of a lambda because lambdas
# "will not evaluate/look up the argument values before it is
# effectively called, breaking situations like using a loop
# variable inside it"
from functools import partial
par = partial(self._update_single_cmd_cb, name=name)
joint_widget.valueChanged.connect(par)
except:
# TODO: Can we do better than swallow the exception?
from sys import exc_info
print 'Unexpected error:', exc_info()[0]
# Enter monitor mode (sending commands disabled)
self._on_jtc_enabled(False)
# Setup ROS interfaces
jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
state_topic = jtc_ns + '/state'
cmd_topic = jtc_ns + '/command'
self._state_sub = rospy.Subscriber(state_topic,
JointTrajectoryControllerState,
self._state_cb,
queue_size=1)
self._cmd_pub = rospy.Publisher(cmd_topic,
JointTrajectory,
queue_size=1)
# Start updating the joint positions
self.jointStateChanged.connect(self._on_joint_state_change)
def _unload_jtc(self):
# Stop updating the joint positions
try:
self.jointStateChanged.disconnect(self._on_joint_state_change)
except:
pass
# Reset ROS interfaces
self._unregister_state_sub()
self._unregister_cmd_pub()
# Clear joint widgets
# NOTE: Implementation is a workaround for:
# https://bugreports.qt-project.org/browse/QTBUG-15990 :(
layout = self._widget.joint_group.layout()
if layout is not None:
while layout.count():
layout.takeAt(0).widget().deleteLater()
# Delete existing layout by reparenting to temporary
QWidget().setLayout(layout)
self._widget.joint_group.setLayout(QFormLayout())
# Reset joint data
self._joint_names = []
self._joint_pos = {}
# Enforce monitor mode (sending commands disabled)
self._widget.enable_button.setChecked(False)
def _running_jtc_info(self):
from controller_manager_msgs.utils\
import filter_by_type, filter_by_state
controller_list = self._list_controllers()
jtc_list = filter_by_type(controller_list,
'JointTrajectoryController',
match_substring=True)
running_jtc_list = filter_by_state(jtc_list, 'running')
return running_jtc_list
def _unregister_cmd_pub(self):
if self._cmd_pub is not None:
self._cmd_pub.unregister()
self._cmd_pub = None
def _unregister_state_sub(self):
if self._state_sub is not None:
self._state_sub.unregister()
self._state_sub = None
def _state_cb(self, msg):
actual_pos = {}
for i in range(len(msg.joint_names)):
joint_name = msg.joint_names[i]
joint_pos = msg.actual.positions[i]
actual_pos[joint_name] = joint_pos
self.jointStateChanged.emit(actual_pos)
def _update_single_cmd_cb(self, val, name):
self._joint_pos[name]['command'] = val
def _update_cmd_cb(self):
dur = []
traj = JointTrajectory()
traj.joint_names = self._joint_names
point = JointTrajectoryPoint()
for name in traj.joint_names:
pos = self._joint_pos[name]['position']
cmd = pos
try:
cmd = self._joint_pos[name]['command']
except (KeyError):
pass
max_vel = self._robot_joint_limits[name]['max_velocity']
dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
point.positions.append(cmd)
point.time_from_start = rospy.Duration(max(dur) / self._speed_scale)
traj.points.append(point)
self._cmd_pub.publish(traj)
def _update_joint_widgets(self):
joint_widgets = self._joint_widgets()
for id in range(len(joint_widgets)):
joint_name = self._joint_names[id]
try:
joint_pos = self._joint_pos[joint_name]['position']
joint_widgets[id].setValue(joint_pos)
except (KeyError):
pass # Can happen when first connected to controller
def _joint_widgets(self): # TODO: Cache instead of compute every time?
widgets = []
layout = self._widget.joint_group.layout()
for row_id in range(layout.rowCount()):
widgets.append(layout.itemAt(row_id,
QFormLayout.FieldRole).widget())
return widgets
class MonitorDashWidget(IconToolButton):
"""
A widget which brings up the rqt_robot_monitor.
Times out after certain period of time (set as 5 sec as of Apr 2013)
without receiving diagnostics msg ('/diagnostics_toplevel_state' of
DiagnosticStatus type), status becomes as 'stale'.
:param context: The plugin context to create the monitor in.
:type context: qt_gui.plugin_context.PluginContext
"""
_msg_trigger = Signal()
def __init__(self, context, icon_paths=[]):
self._graveyard = []
ok_icon = ['bg-green.svg', 'ic-diagnostics.svg']
warn_icon = ['bg-yellow.svg', 'ic-diagnostics.svg',
'ol-warn-badge.svg']
err_icon = ['bg-red.svg', 'ic-diagnostics.svg', 'ol-err-badge.svg']
stale_icon = ['bg-grey.svg', 'ic-diagnostics.svg',
'ol-stale-badge.svg']
icons = [ok_icon, warn_icon, err_icon, stale_icon]
super(MonitorDashWidget, self).__init__('MonitorWidget', icons,
icon_paths=icon_paths)
self.setFixedSize(self._icons[0].actualSize(QSize(50, 30)))
self._monitor = None
self._close_mutex = QMutex()
self._show_mutex = QMutex()
self._last_update = rospy.Time.now()
self.context = context
self.clicked.connect(self._show_monitor)
self._monitor_shown = False
self.setToolTip('Diagnostics')
self._diagnostics_toplevel_state_sub = rospy.Subscriber(
'diagnostics_toplevel_state',
DiagnosticStatus, self.toplevel_state_callback)
self._top_level_state = -1
self._stall_timer = QTimer()
self._stall_timer.timeout.connect(self._stalled)
self._stalled()
self._plugin_settings = None
self._instance_settings = None
self._msg_trigger.connect(self._handle_msg_trigger)
def toplevel_state_callback(self, msg):
self._is_stale = False
self._msg_trigger.emit()
if self._top_level_state != msg.level:
if (msg.level >= 2):
self.update_state(2)
self.setToolTip("Diagnostics: Error")
elif (msg.level == 1):
self.update_state(1)
self.setToolTip("Diagnostics: Warning")
else:
self.update_state(0)
self.setToolTip("Diagnostics: OK")
self._top_level_state = msg.level
def _handle_msg_trigger(self):
self._stall_timer.start(5000)
def _stalled(self):
self._stall_timer.stop()
self._is_stale = True
self.update_state(3)
self._top_level_state = 3
self.setToolTip("Diagnostics: Stale\nNo message received on "
"/diagnostics_agg in the last 5 seconds")
def _show_monitor(self):
with QMutexLocker(self._show_mutex):
try:
if self._monitor_shown:
self.context.remove_widget(self._monitor)
self._monitor_close()
self._monitor_shown = False
else:
self._monitor = RobotMonitorWidget(self.context,
'/diagnostics_agg')
if self._plugin_settings:
self._monitor.restore_settings(self._plugin_settings,
self._instance_settings)
self.context.add_widget(self._monitor)
self._monitor_shown = True
except Exception:
if self._monitor_shown == False:
raise
#TODO: when closeEvents is available fix this hack
# (It ensures the button will toggle correctly)
self._monitor_shown = False
self._show_monitor()
def _monitor_close(self):
if self._monitor_shown:
with QMutexLocker(self._close_mutex):
if self._plugin_settings:
self._monitor.save_settings(self._plugin_settings,
self._instance_settings)
self._monitor.shutdown()
self._monitor.close()
self._graveyard.append(self._monitor)
self._monitor = None
def shutdown_widget(self):
self._stall_timer.stop()
if self._monitor:
self._monitor.shutdown()
self._diagnostics_toplevel_state_sub.unregister()
def save_settings(self, plugin_settings, instance_settings):
if self._monitor_shown:
self._monitor.save_settings(self._plugin_settings,
self._instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._plugin_settings = plugin_settings
self._instance_settings = instance_settings
class Top(Plugin):
NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads']
OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ]
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ]
SORT_TYPE = [str, str, float, float, float ]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.iteritems():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
#self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0)
twi.setHidden(len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex', int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class MotionEditorWidget(QWidget):
position_renamed = Signal(QListWidgetItem)
def __init__(self, motion_publisher, robot_config):
super(MotionEditorWidget, self).__init__()
self.robot_config = robot_config
self._motion_publisher = motion_publisher
self._motion_data = MotionData(robot_config)
self._filter_pattern = ''
self._playback_marker = None
self._playback_timer = None
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'motion_editor.ui')
loadUi(ui_file, self)
self.list_widgets = {}
for group_type in robot_config.group_types():
list_widget = QListWidget()
list_widget.setSortingEnabled(True)
list_widget.setDragDropMode(QAbstractItemView.DragOnly)
list_widget.setContextMenuPolicy(Qt.CustomContextMenu)
list_widget.customContextMenuRequested.connect(
lambda pos, _group_type=group_type: self.
positions_list_context_menu(_group_type, pos))
list_widget.itemChanged.connect(self.on_list_item_changed)
self.position_lists_layout.addWidget(list_widget)
self.list_widgets[group_type] = list_widget
self._timeline_widget = TimelineWidget()
for track_name in self.robot_config.sorted_groups():
track_type = self.robot_config.groups[track_name].group_type
track = self._timeline_widget.add_track(track_name, track_type)
list_widget = self.list_widgets[track_type]
palette = list_widget.palette()
palette.setColor(QPalette.Base, track._colors['track'])
list_widget.setPalette(palette)
self.timeline_group.layout().addWidget(self._timeline_widget)
for group_type in robot_config.group_types():
label = QLabel(group_type)
label.setAlignment(Qt.AlignHCenter | Qt.AlignVCenter)
self.group_label_layout.addWidget(label)
self.update_motion_name_combo()
self.stop_motion_button.pressed.connect(self.on_motion_stop_pressed)
def on_list_item_changed(self, item):
print 'Position name changed from', item._text, 'to', item.text()
self.position_renamed.emit(item)
def on_motion_stop_pressed(self):
self._clear_playback_marker()
self._motion_publisher.stop_motion()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('splitter', self.splitter.saveState())
instance_settings.set_value('filter_pattern',
self.filter_pattern_edit.text())
instance_settings.set_value('filter_motions_checked',
self.filter_motions_check.isChecked())
instance_settings.set_value('filter_positions_checked',
self.filter_positions_check.isChecked())
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.contains('splitter'):
self.splitter.restoreState(instance_settings.value('splitter'))
else:
self.splitter.setSizes([300, 100])
self.filter_pattern_edit.setText(
instance_settings.value('filter_pattern', ''))
self.filter_motions_check.setChecked(
instance_settings.value('filter_motions_checked', False) in (
1, True, 'true'))
self.filter_positions_check.setChecked(
instance_settings.value('filter_positions_checked', False) in (
1, True, 'true'))
@Slot()
def on_clear_motion_button_clicked(self):
self._timeline_widget.scene().clear()
@Slot()
def on_delete_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error', 'No motion selected to delete.')
return
if motion_name not in self._motion_data:
QMessageBox.warning(
self, 'Error', 'Motion "%s" not in motion list.' % motion_name)
return
self._motion_data.delete(motion_name)
self.update_motion_name_combo()
@Slot()
def on_save_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error',
'No name given to save this motion.')
return
self._motion_data.save(motion_name, self.get_motion_from_timeline())
self.update_motion_name_combo()
@Slot()
def on_load_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error', 'No motion selected to load.')
return
if motion_name not in self._motion_data:
QMessageBox.warning(
self, 'Error', 'Motion "%s" not in motion list.' % motion_name)
return
self.on_clear_motion_button_clicked()
motion = self._motion_data[motion_name]
for group_name, poses in motion.items():
for pose in poses:
data = self.robot_config.groups[group_name].adapt_to_side(
pose['positions'])
self._timeline_widget.scene().add_clip(group_name,
pose['name'],
pose['starttime'],
pose['duration'], data)
@Slot()
def on_null_motion_button_clicked(self):
self._clear_playback_marker()
for group_name in self.robot_config.groups:
target_position = [0] * len(
self.robot_config.groups[group_name].joints)
self._motion_publisher.move_to_position(
group_name, target_position, self.time_factor_spin.value())
@Slot()
def on_run_motion_button_clicked(self):
motion_name = str(self.motion_name_combo.currentText())
if len(motion_name) == 0:
QMessageBox.warning(self, 'Error', 'No motion selected to run.')
return
if motion_name not in self._motion_data:
QMessageBox.warning(
self, 'Error', 'Motion "%s" not in motion list.' % motion_name)
return
self._clear_playback_marker()
self._motion_publisher.publish_motion(self._motion_data[motion_name],
self.time_factor_spin.value())
print '[Motion Editor] Running motion:', motion_name
@Slot(str)
def on_filter_pattern_edit_textChanged(self, pattern):
self._filter_pattern = pattern
self._apply_filter_to_position_lists()
self.update_motion_name_combo()
def _apply_filter_to_position_lists(self):
for group_type in self.robot_config.group_types():
list_widget = self.list_widgets[group_type]
for row in range(list_widget.count()):
item = list_widget.item(row)
hidden = self.filter_positions_check.isChecked() and re.search(
self._filter_pattern, item.text()) is None
item.setHidden(hidden)
@Slot(bool)
def on_filter_positions_check_toggled(self, checked):
self._apply_filter_to_position_lists()
@Slot(bool)
def on_filter_motions_check_toggled(self, checked):
self.update_motion_name_combo()
def update_motion_name_combo(self):
combo = self.motion_name_combo
# remember selected motion name
motion_name = str(combo.currentText())
# update motion names
combo.clear()
motion_names = self._motion_data.keys()
if self.filter_motions_check.isChecked():
motion_names = [
name for name in motion_names
if re.search(self._filter_pattern, name) is not None
]
combo.addItems(sorted(motion_names))
# restore previously selected motion name
motion_index = combo.findText(motion_name)
combo.setCurrentIndex(motion_index)
def update_positions_lists(self, positions):
for group_type in self.robot_config.group_types():
list_widget = self.list_widgets[group_type]
list_widget.clear()
for name, position in positions[group_type].items():
item = QListWidgetItem(name)
item._data = position
item._text = name
item._type = group_type
item.setFlags(item.flags() | Qt.ItemIsEditable)
list_widget.addItem(item)
self._apply_filter_to_position_lists()
def positions_list_context_menu(self, group_type, pos):
list_widget = self.list_widgets[group_type]
list_item = list_widget.itemAt(pos)
if list_item is None:
return
menu = QMenu()
move_to = {}
for group in self.robot_config.group_list():
if list_item._type == group.group_type:
move_to[menu.addAction('move "%s"' %
group.name)] = [group.name]
# move_to[menu.addAction('move all')] = list(move_to.itervalues())
move_to[menu.addAction('move all')] = [
group_list[0] for group_list in list(move_to.itervalues())
]
result = menu.exec_(list_widget.mapToGlobal(pos))
if result in move_to:
group_names = move_to[result]
for group_name in group_names:
target_positions = self.robot_config.groups[
group_name].adapt_to_side(list_item._data)
self._motion_publisher.move_to_position(
group_name, target_positions,
self.time_factor_spin.value())
print '[Motion Editor] Moving %s to: %s' % (
group_name,
zip(self.robot_config.groups[group_name].joints_sorted(),
target_positions))
def get_motion_from_timeline(self):
motion = {}
for group_name, clips in self._timeline_widget.scene().clips().items():
motion[group_name] = []
for clip in clips:
pose = {
'name':
clip.text(),
'starttime':
clip.starttime(),
'duration':
clip.duration(),
'positions':
self.robot_config.groups[group_name].adapt_to_side(
clip.data()),
}
motion[group_name].append(pose)
return motion
@Slot()
def on_run_timeline_button_clicked(self):
print '[Motion Editor] Running timeline.'
self._playback_duration = 0.0
for clips in self._timeline_widget.scene().clips().values():
if len(clips) > 0 and self._playback_duration < clips[-1].endtime(
):
self._playback_duration = clips[-1].endtime()
self._playback_time_factor = self.time_factor_spin.value()
self._clear_playback_marker()
self._playback_marker = self._timeline_widget.scene().add_marker(0.0)
self._playback_timer = QTimer()
self._playback_timer.setInterval(30)
self._playback_timer.timeout.connect(self._playback_update)
self._playback_start = rospy.get_time()
self._motion_publisher.publish_motion(self.get_motion_from_timeline(),
self.time_factor_spin.value())
self._playback_timer.start()
def _clear_playback_marker(self):
if self._playback_timer is not None:
self._playback_timer.stop()
if self._playback_marker is not None:
self._playback_marker.remove()
@Slot()
def _playback_update(self):
duration = (rospy.get_time() -
self._playback_start) / self._playback_time_factor
if duration > self._playback_duration:
self._clear_playback_marker()
else:
self._playback_marker.set_time(duration)
class BagTimeline(QGraphicsScene):
"""
BagTimeline contains bag files, all information required to display the bag data visualization on the screen
Also handles events
"""
status_bar_changed_signal = Signal()
selected_region_changed = Signal(rospy.Time, rospy.Time)
def __init__(self, context, publish_clock):
"""
:param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext''
"""
super(BagTimeline, self).__init__()
self._bags = []
self._bag_lock = threading.RLock()
self.background_task = None # Display string
self.background_task_cancel = False
# Playing / Recording
self._playhead_lock = threading.RLock()
self._max_play_speed = 1024.0 # fastest X play speed
self._min_play_speed = 1.0 / 1024.0 # slowest X play speed
self._play_speed = 0.0
self._play_all = False
self._playhead_positions_cvs = {}
self._playhead_positions = {} # topic -> (bag, position)
self._message_loaders = {}
self._messages_cvs = {}
self._messages = {} # topic -> (bag, msg_data)
self._message_listener_threads = {
} # listener -> MessageListenerThread
self._player = False
self._publish_clock = publish_clock
self.last_frame = None
self.last_playhead = None
self.desired_playhead = None
self.wrap = True # should the playhead wrap when it reaches the end?
self.stick_to_end = False # should the playhead stick to the end?
self._play_timer = QTimer()
self._play_timer.timeout.connect(self.on_idle)
self._play_timer.setInterval(3)
# Plugin popup management
self._context = context
self.popups = {}
self._views = []
self._listeners = {}
# Initialize scene
# the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast.
# otherwise a dark qt theme will default it to black and the frame render pen will be unreadable
self.setBackgroundBrush(Qt.white)
self._timeline_frame = TimelineFrame(self)
self._timeline_frame.setPos(0, 0)
self.addItem(self._timeline_frame)
self.background_progress = 0
self.__closed = False
def get_context(self):
"""
:returns: the ROS_GUI context, 'PluginContext'
"""
return self._context
def handle_close(self):
"""
Cleans up the timeline, bag and any threads
"""
if self.__closed:
return
else:
self.__closed = True
self._play_timer.stop()
for topic in self._get_topics():
self.stop_publishing(topic)
self._message_loaders[topic].stop()
if self._player:
self._player.stop()
if self.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 = [
bag
] # actually, only *one* bag can be used at a time (contrary to original rqt_bag)
bag_topics = bag_helper.get_topics(bag)
new_topics = set(bag_topics) - set(self._timeline_frame.topics)
for topic in new_topics:
qWarning("Adding topic %s" % topic)
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(
)
self._timeline_frame.set_renderers_active(
True) # enable the thumbnails for image streams
# 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
### Views / listeners
def add_view(self, topic, frame):
self._views.append(frame)
def has_listeners(self, topic):
return topic in self._listeners
def add_listener(self, topic, listener):
self._listeners.setdefault(topic, []).append(listener)
self._message_listener_threads[(topic,
listener)] = MessageListenerThread(
self, topic, listener)
# Notify the message listeners
self._message_loaders[topic].reset()
with self._playhead_positions_cvs[topic]:
self._playhead_positions_cvs[topic].notify_all()
self.update()
def remove_listener(self, topic, listener):
topic_listeners = self._listeners.get(topic)
if topic_listeners is not None and listener in topic_listeners:
topic_listeners.remove(listener)
if len(topic_listeners) == 0:
del self._listeners[topic]
# Stop the message listener thread
if (topic, listener) in self._message_listener_threads:
self._message_listener_threads[(topic, listener)].stop()
del self._message_listener_threads[(topic, listener)]
self.update()
### Playhead
# property: play_speed
def _get_play_speed(self):
if self._timeline_frame._paused:
return 0.0
return self._play_speed
def _set_play_speed(self, play_speed):
if play_speed == self._play_speed:
return
if play_speed > 0.0:
self._play_speed = min(self._max_play_speed,
max(self._min_play_speed, play_speed))
elif play_speed < 0.0:
self._play_speed = max(-self._max_play_speed,
min(-self._min_play_speed, play_speed))
else:
self._play_speed = play_speed
if self._play_speed < 1.0:
self.stick_to_end = False
self.update()
play_speed = property(_get_play_speed, _set_play_speed)
def toggle_play(self):
if self._play_speed != 0.0:
self.play_speed = 0.0
else:
self.play_speed = 1.0
def navigate_play(self):
self.play_speed = 1.0
self.last_frame = rospy.Time.from_sec(time.time())
self.last_playhead = self._timeline_frame.playhead
self._play_timer.start()
def navigate_stop(self):
self.play_speed = 0.0
self._play_timer.stop()
def navigate_previous(self):
self.navigate_stop()
self._timeline_frame.playhead = self.get_previous_message_time()
self.last_playhead = self._timeline_frame.playhead
def navigate_next(self):
self.navigate_stop()
self._timeline_frame.playhead = self.get_next_message_time()
self.last_playhead = self._timeline_frame.playhead
def navigate_rewind(self):
if self._play_speed < 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = -1.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
def navigate_fastforward(self):
if self._play_speed > 0.0:
new_play_speed = self._play_speed * 2.0
elif self._play_speed == 0.0:
new_play_speed = 2.0
else:
new_play_speed = self._play_speed * 0.5
self.play_speed = new_play_speed
def navigate_start(self):
self._timeline_frame.playhead = self._timeline_frame.play_region[0]
def navigate_end(self):
self._timeline_frame.playhead = self._timeline_frame.play_region[1]
class CommandWidget(QWidget):
def __init__(self, node):
super(CommandWidget, self).__init__()
self.setObjectName('CommandWidget')
self.node = node
self.REDRAW_INTERVAL = 30
self.PUBLISH_INTERVAL = 100
self.CMD_VEL_X_FACTOR = 1000.0
self.CMD_VEL_YAW_FACTOR = -10.0
pkg_name = 'oroca_rqt_command'
ui_filename = 'rqt_command.ui'
topic_name = 'cmd_vel'
service_name = 'led_control'
_, package_path = get_resource('packages', pkg_name)
ui_file = os.path.join(package_path, 'share', pkg_name, 'resource',
ui_filename)
loadUi(ui_file, self)
self.pub_velocity = Twist()
self.pub_velocity.linear.x = 0.0
self.pub_velocity.angular.z = 0.0
self.sub_velocity = Twist()
self.sub_velocity.linear.x = 0.0
self.sub_velocity.angular.z = 0.0
self.slider_x.setValue(0)
self.lcd_number_x.display(0.0)
self.lcd_number_yaw.display(0.0)
qos = QoSProfile(depth=10)
self.publisher = self.node.create_publisher(Twist, topic_name, qos)
self.subscriber = self.node.create_subscription(
Twist, topic_name, self.get_velocity, qos)
self.service_server = self.node.create_service(SetBool, service_name,
self.set_led_status)
self.service_client = self.node.create_client(SetBool, service_name)
self.publish_timer = QTimer(self)
self.publish_timer.timeout.connect(self.send_velocity)
self.publish_timer.start(self.PUBLISH_INTERVAL)
self.update_timer = QTimer(self)
self.update_timer.timeout.connect(self.update_indicators)
self.update_timer.start(self.REDRAW_INTERVAL)
self.push_button_w.pressed.connect(self.increase_linear_x)
self.push_button_x.pressed.connect(self.decrease_linear_x)
self.push_button_a.pressed.connect(self.increase_angular_z)
self.push_button_d.pressed.connect(self.decrease_angular_z)
self.push_button_s.pressed.connect(self.set_stop)
self.push_button_w.setShortcut('w')
self.push_button_x.setShortcut('x')
self.push_button_a.setShortcut('a')
self.push_button_d.setShortcut('d')
self.push_button_s.setShortcut('s')
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space), self)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self.push_button_s.pressed)
self.radio_button_led_on.clicked.connect(self.call_led_service)
self.radio_button_led_off.clicked.connect(self.call_led_service)
self.radio_button_led_on.setShortcut('o')
self.radio_button_led_off.setShortcut('f')
def get_velocity(self, msg):
self.sub_velocity = msg
def set_led_status(self, request, response):
if request.data:
self.push_button_led_status.setText('ON')
self.push_button_led_status.setStyleSheet(
'color: rgb(255, 170, 0);')
response.success = True
response.message = 'LED ON'
elif not request.data:
self.push_button_led_status.setText('OFF')
self.push_button_led_status.setStyleSheet('')
response.success = True
response.message = 'LED OFF'
else:
response.success = False
return response
def increase_linear_x(self):
self.pub_velocity.linear.x += 0.1
def decrease_linear_x(self):
self.pub_velocity.linear.x -= 0.1
def increase_angular_z(self):
self.pub_velocity.angular.z += 0.1
def decrease_angular_z(self):
self.pub_velocity.angular.z -= 0.1
def set_stop(self):
self.pub_velocity.linear.x = 0.0
self.pub_velocity.angular.z = 0.0
def call_led_service(self):
request = SetBool.Request()
if self.radio_button_led_on.isChecked():
request.data = True
elif self.radio_button_led_off.isChecked():
request.data = False
wait_count = 1
while not self.service_client.wait_for_service(timeout_sec=0.5):
if wait_count > 5:
return
self.node.get_logger().error(
'Service not available #{0}'.format(wait_count))
wait_count += 1
future = self.service_client.call_async(request)
while rclpy.ok():
if future.done():
if future.result() is not None:
response = future.result()
self.node.get_logger().info(
'Result of service call: {0}'.format(response.message))
else:
self.node.get_logger().error('Error calling service')
break
def send_velocity(self):
twist = Twist()
twist.linear.x = self.pub_velocity.linear.x
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = self.pub_velocity.angular.z
self.publisher.publish(twist)
def update_indicators(self):
self.slider_x.setValue(self.sub_velocity.linear.x *
self.CMD_VEL_X_FACTOR)
self.dial_yaw.setValue(self.sub_velocity.angular.z *
self.CMD_VEL_YAW_FACTOR)
self.lcd_number_x.display(self.sub_velocity.linear.x)
self.lcd_number_yaw.display(self.sub_velocity.angular.z)
def shutdown_widget(self):
self.update_timer.stop()
self.publish_timer.stop()
self.node.destroy_client(self.service_client)
self.node.destroy_service(self.service_server)
self.node.destroy_subscription(self.subscriber)
self.node.destroy_publisher(self.publisher)
class PoseViewWidget(QWidget):
def __init__(self, plugin):
super(PoseViewWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_pose_view'), 'resource', 'PoseViewWidget.ui')
loadUi(ui_file, self)
self._plugin = plugin
self._position = (2.0, 2.0, 2.0)
self._orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
self._topic_name = None
self._subscriber = None
# create GL view
self._gl_view = GLWidget()
self._gl_view.setAcceptDrops(True)
# backup and replace original paint method
self._gl_view.paintGL_original = self._gl_view.paintGL
self._gl_view.paintGL = self._gl_view_paintGL
# backup and replace original mouse release method
self._gl_view.mouseReleaseEvent_original = self._gl_view.mouseReleaseEvent
self._gl_view.mouseReleaseEvent = self._gl_view_mouseReleaseEvent
# add GL view to widget layout
self.layout().addWidget(self._gl_view)
# init and start update timer with 40ms (25fps)
self._update_timer = QTimer(self)
self._update_timer.timeout.connect(self.update_timeout)
self._update_timer.start(40)
def save_settings(self, plugin_settings, instance_settings):
view_matrix_string = repr(self._gl_view.get_view_matrix())
instance_settings.set_value('view_matrix', view_matrix_string)
def restore_settings(self, plugin_settings, instance_settings):
view_matrix_string = instance_settings.value('view_matrix')
try:
view_matrix = eval(view_matrix_string)
except Exception:
view_matrix = None
if view_matrix is not None:
self._gl_view.set_view_matrix(view_matrix)
else:
self._set_default_view()
def _set_default_view(self):
self._gl_view.makeCurrent()
self._gl_view.reset_view()
self._gl_view.rotate((0, 0, 1), 45)
self._gl_view.rotate((1, 0, 0), -65)
self._gl_view.translate((0, -3, -15))
def update_timeout(self):
self._gl_view.makeCurrent()
self._gl_view.updateGL()
def _gl_view_paintGL(self):
self._gl_view.paintGL_original()
self._paintGLGrid()
self._paintGLCoorsystem()
self._paintGLBox()
def _paintGLBox(self):
# FIXME: add user configurable setting to allow use of translation as well
self._position = (2.0, 2.0, 2.0) # Set fixed translation for now
glTranslatef(*self._position) # Translate Box
matrix = quaternion_matrix(self._orientation) # convert quaternion to translation matrix
glMultMatrixf(matrix) # Rotate Box
glBegin(GL_QUADS) # Start Drawing The Box
glColor3f(0.0, 1.0, 0.0)
glVertex3f(1.0, 1.0, -1.0) # Top Right Of The Quad (Top)
glVertex3f(-1.0, 1.0, -1.0) # Top Left Of The Quad (Top)
glVertex3f(-1.0, 1.0, 1.0) # Bottom Left Of The Quad (Top)
glVertex3f(1.0, 1.0, 1.0) # Bottom Right Of The Quad (Top)
glColor3f(0.5, 1.0, 0.5)
glVertex3f(1.0, -1.0, 1.0) # Top Right Of The Quad (Bottom)
glVertex3f(-1.0, -1.0, 1.0) # Top Left Of The Quad (Bottom)
glVertex3f(-1.0, -1.0, -1.0) # Bottom Left Of The Quad (Bottom)
glVertex3f(1.0, -1.0, -1.0) # Bottom Right Of The Quad (Bottom)
glColor3f(0.0, 0.0, 1.0)
glVertex3f(1.0, 1.0, 1.0) # Top Right Of The Quad (Front)
glVertex3f(-1.0, 1.0, 1.0) # Top Left Of The Quad (Front)
glVertex3f(-1.0, -1.0, 1.0) # Bottom Left Of The Quad (Front)
glVertex3f(1.0, -1.0, 1.0) # Bottom Right Of The Quad (Front)
glColor3f(0.5, 0.5, 1.0)
glVertex3f(1.0, -1.0, -1.0) # Bottom Left Of The Quad (Back)
glVertex3f(-1.0, -1.0, -1.0) # Bottom Right Of The Quad (Back)
glVertex3f(-1.0, 1.0, -1.0) # Top Right Of The Quad (Back)
glVertex3f(1.0, 1.0, -1.0) # Top Left Of The Quad (Back)
glColor3f(1.0, 0.5, 0.5)
glVertex3f(-1.0, 1.0, 1.0) # Top Right Of The Quad (Left)
glVertex3f(-1.0, 1.0, -1.0) # Top Left Of The Quad (Left)
glVertex3f(-1.0, -1.0, -1.0) # Bottom Left Of The Quad (Left)
glVertex3f(-1.0, -1.0, 1.0) # Bottom Right Of The Quad (Left)
glColor3f(1.0, 0.0, 0.0)
glVertex3f(1.0, 1.0, -1.0) # Top Right Of The Quad (Right)
glVertex3f(1.0, 1.0, 1.0) # Top Left Of The Quad (Right)
glVertex3f(1.0, -1.0, 1.0) # Bottom Left Of The Quad (Right)
glVertex3f(1.0, -1.0, -1.0) # Bottom Right Of The Quad (Right)
glEnd() # Done Drawing The Quad
def _paintGLGrid(self):
resolution_millimeters = 1
gridded_area_size = 100
glLineWidth(1.0)
glBegin(GL_LINES)
glColor3f(1.0, 1.0, 1.0)
glVertex3f(gridded_area_size, 0, 0)
glVertex3f(-gridded_area_size, 0, 0)
glVertex3f(0, gridded_area_size, 0)
glVertex3f(0, -gridded_area_size, 0)
num_of_lines = int(gridded_area_size / resolution_millimeters)
for i in range(num_of_lines):
glVertex3f(resolution_millimeters * i, -gridded_area_size, 0)
glVertex3f(resolution_millimeters * i, gridded_area_size, 0)
glVertex3f(gridded_area_size, resolution_millimeters * i, 0)
glVertex3f(-gridded_area_size, resolution_millimeters * i, 0)
glVertex3f(resolution_millimeters * (-i), -gridded_area_size, 0)
glVertex3f(resolution_millimeters * (-i), gridded_area_size, 0)
glVertex3f(gridded_area_size, resolution_millimeters * (-i), 0)
glVertex3f(-gridded_area_size, resolution_millimeters * (-i), 0)
glEnd()
def _paintGLCoorsystem(self):
glLineWidth(10.0)
glBegin(GL_LINES)
glColor3f(1.0, 0.0, 0.0)
glVertex3f(0.0, 0.0, 0.0)
glVertex3f(1.0, 0.0, 0.0)
glColor3f(0.0, 1.0, 0.0)
glVertex3f(0.0, 0.0, 0.0)
glVertex3f(0.0, 1.0, 0.0)
glColor3f(0.0, 0.0, 1.0)
glVertex3f(0.0, 0.0, 0.0)
glVertex3f(0.0, 0.0, 1.0)
glEnd()
def _gl_view_mouseReleaseEvent(self, event):
if event.button() == Qt.RightButton:
menu = QMenu(self._gl_view)
action = QAction(self._gl_view.tr("Reset view"), self._gl_view)
menu.addAction(action)
action.triggered.connect(self._set_default_view)
menu.exec_(self._gl_view.mapToGlobal(event.pos()))
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
if len(topic_name) == 0:
qWarning('PoseViewWidget.dragEnterEvent(): event.mimeData() text is empty')
return
else:
if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
qWarning('PoseViewWidget.dragEnterEvent(): event.source() has no attribute selectedItems or length of selectedItems is 0')
return
item = event.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name is None:
qWarning('PoseViewWidget.dragEnterEvent(): selectedItem has no UserRole data with a topic name')
return
# check for valid topic
msg_class, self._topic_name, _ = get_topic_class(topic_name)
if msg_class is None:
qWarning('PoseViewWidget.dragEnterEvent(): No message class was found for topic "%s".' % topic_name)
return
# check for valid message class
quaternion_slot_path, point_slot_path = self._get_slot_paths(msg_class)
if quaternion_slot_path is None and point_slot_path is None:
qWarning('PoseViewWidget.dragEnterEvent(): No Pose, Quaternion or Point data was found outside of arrays in "%s" on topic "%s".'
% (msg_class._type, topic_name))
return
event.acceptProposedAction()
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
dropped_item = event.source().selectedItems()[0]
topic_name = str(dropped_item.data(0, Qt.UserRole))
self._unregister_topic()
self._subscribe_topic(topic_name)
def _unregister_topic(self):
if self._subscriber:
self._subscriber.unregister()
@staticmethod
def _make_path_list_from_path_string(path):
path = path.split('/')
if path == ['']:
return []
return path
@staticmethod
def _get_slot_paths(msg_class):
# find first Pose in msg_class
pose_slot_paths = find_slots_by_type_bfs(msg_class, Pose)
for path in pose_slot_paths:
# make sure the path does not contain an array, because we don't want to deal with empty arrays...
if '[' not in path:
path = PoseViewWidget._make_path_list_from_path_string(pose_slot_paths[0])
return path + ['orientation'], path + ['position']
# if no Pose is found, find first Quaternion and Point
quaternion_slot_paths = find_slots_by_type_bfs(msg_class, Quaternion)
for path in quaternion_slot_paths:
if '[' not in path:
quaternion_slot_path = PoseViewWidget._make_path_list_from_path_string(path)
break
else:
quaternion_slot_path = None
point_slot_paths = find_slots_by_type_bfs(msg_class, Point)
for path in point_slot_paths:
if '[' not in path:
point_slot_path = PoseViewWidget._make_path_list_from_path_string(path)
break
else:
point_slot_path = None
return quaternion_slot_path, point_slot_path
def _subscribe_topic(self, topic_name):
msg_class, self._topic_name, _ = get_topic_class(topic_name)
quaternion_slot_path, point_slot_path = self._get_slot_paths(msg_class)
self._subscriber = rospy.Subscriber(
self._topic_name,
msg_class,
self.message_callback,
callback_args=(quaternion_slot_path, point_slot_path)
)
def message_callback(self, message, slot_paths):
quaternion_slot_path = slot_paths[0]
point_slot_path = slot_paths[1]
if quaternion_slot_path is None:
self._orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
else:
orientation = message
for slot_name in quaternion_slot_path:
orientation = getattr(orientation, slot_name)
self._orientation = (orientation.x, orientation.y, orientation.z, orientation.w)
if point_slot_path is None:
# if no point is given, set it to a fixed offset so the axes can be seen
self._position = (2.0, 2.0, 2.0)
else:
position = message
for slot_name in point_slot_path:
position = getattr(position, slot_name)
self._position = (position.x, position.y, position.z)
def shutdown_plugin(self):
self._unregister_topic()
class PlotWidget(QWidget):
_redraw_interval = 40
def __init__(self, arguments=None, initial_topics=None):
super(PlotWidget, self).__init__()
self.setObjectName('PlotWidget')
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'plot.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = None
self.subscribe_topic_button.setEnabled(False)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
# save command line arguments
self._arguments = arguments
self._initial_topics = initial_topics
def switch_data_plot_widget(self, data_plot):
self.enable_timer(enabled=False)
self.data_plot_layout.removeWidget(self.data_plot)
if self.data_plot is not None:
self.data_plot.close()
self.data_plot = data_plot
self.data_plot_layout.addWidget(self.data_plot)
# setup drag 'n drop
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
if self._initial_topics:
for topic_name in self._initial_topics:
self.add_topic(topic_name)
self._initial_topics = None
else:
for topic_name, rosdata in self._rosdata.items():
data_x, data_y = rosdata.next()
self.data_plot.add_curve(topic_name, topic_name, data_x,
data_y)
self._subscribed_topics_changed()
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
# get topic name
if not event.mimeData().hasText():
if not hasattr(event.source(), 'selectedItems') or len(
event.source().selectedItems()) == 0:
qWarning(
'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0'
)
return
item = event.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name == None:
qWarning(
'Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)'
)
return
else:
topic_name = str(event.mimeData().text())
# check for numeric field type
is_numeric, is_array, message = is_slot_numeric(topic_name)
if is_numeric and not is_array:
event.acceptProposedAction()
else:
qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.add_topic(topic_name)
@Slot(str)
def on_topic_edit_textChanged(self, topic_name):
# on empty topic name, update topics
if topic_name in ('', '/'):
self._topic_completer.update_topics()
is_numeric, is_array, message = is_slot_numeric(topic_name)
self.subscribe_topic_button.setEnabled(is_numeric and not is_array)
self.subscribe_topic_button.setToolTip(message)
@Slot()
def on_subscribe_topic_button_clicked(self):
self.add_topic(str(self.topic_edit.text()))
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
@Slot()
def on_clear_button_clicked(self):
self.clean_up_subscribers()
def update_plot(self):
if self.data_plot is not None:
for topic_name, rosdata in self._rosdata.items():
try:
data_x, data_y = rosdata.next()
self.data_plot.update_values(topic_name, data_x, data_y)
except RosPlotException as e:
qWarning('PlotWidget.update_plot(): error in rosplot: %s' %
e)
self.data_plot.redraw()
def _subscribed_topics_changed(self):
self._update_remove_topic_menu()
if self._arguments:
if self._arguments.start_paused:
self.pause_button.setChecked(True)
if not self.pause_button.isChecked():
# if pause button is not pressed, enable timer based on subscribed topics
self.enable_timer(self._rosdata)
def _update_remove_topic_menu(self):
def make_remove_topic_function(x):
return lambda: self.remove_topic(x)
self._remove_topic_menu.clear()
for topic_name in sorted(self._rosdata.keys()):
action = QAction(topic_name, self._remove_topic_menu)
action.triggered.connect(make_remove_topic_function(topic_name))
self._remove_topic_menu.addAction(action)
self.remove_topic_button.setMenu(self._remove_topic_menu)
def add_topic(self, topic_name):
if topic_name in self._rosdata:
qWarning('PlotWidget.add_topic(): topic already subscribed: %s' %
topic_name)
return
self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
data_x, data_y = self._rosdata[topic_name].next()
self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)
self._subscribed_topics_changed()
def remove_topic(self, topic_name):
self._rosdata[topic_name].close()
del self._rosdata[topic_name]
self.data_plot.remove_curve(topic_name)
self._subscribed_topics_changed()
def clean_up_subscribers(self):
for topic_name, rosdata in self._rosdata.items():
rosdata.close()
self.data_plot.remove_curve(topic_name)
self._rosdata = {}
self._subscribed_topics_changed()
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
class ROSData(object):
"""
Subscriber to ROS topic that buffers incoming data
"""
def __init__(self, topic_code, topic_item, start_time):
self.name = topic_code + '/' + topic_item
self.start_time = start_time
self.error = None
self.lock = threading.Lock()
self.buff_x = []
self.buff_y = []
self.interval = 100 # in milliseconds, period of regular update
self.timer = QTimer()
self.timer.setInterval(self.interval)
self.code = topic_code
self.item = topic_item
# go through options and decide what your self.data will be, given the ros subscribers
if topic_code == 's':
if topic_item == 'chi':
self.timer.timeout.connect(self.state_chi_cb)
elif topic_item == 'phi':
self.timer.timeout.connect(self.state_phi_cb)
elif topic_item == 'theta':
self.timer.timeout.connect(self.state_theta_cb)
elif topic_item == 'Va':
self.timer.timeout.connect(self.state_Va_cb)
elif topic_code == 'ci':
if topic_item == 'phi_c':
self.timer.timeout.connect(self.conin_phi_c_cb)
elif topic_item == 'theta_c':
self.timer.timeout.connect(self.conin_theta_c_cb)
elif topic_code == 'cc':
if topic_item == 'chi_c':
self.timer.timeout.connect(self.concom_chi_c_cb)
elif topic_item == 'Va_c':
self.timer.timeout.connect(self.concom_Va_c_cb)
self.timer.start()
def close(self):
self.timer.stop()
def state_chi_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(StateSub.chi)
def state_phi_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(StateSub.phi)
def state_theta_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(StateSub.theta)
def state_Va_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(StateSub.Va)
def conin_phi_c_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(ConInSub.phi_c)
def conin_theta_c_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(ConInSub.theta_c)
def concom_chi_c_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(ConComSub.chi_c)
def concom_Va_c_cb(self):
self.buff_x.append(rospy.get_time() - self.start_time)
self.buff_y.append(ConComSub.Va_c)
def __next__(self):
"""
Get the next data in the series
:returns: [xdata], [ydata]
"""
if self.error:
raise self.error
try:
self.lock.acquire()
buff_x = self.buff_x
buff_y = self.buff_y
self.buff_x = []
self.buff_y = []
finally:
self.lock.release()
return buff_x, buff_y
class MyPlugin(Plugin):
def __init__(self, context):
super(MyPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MyPlugin')
# 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
# ROS Publisher
self._publisher = None
robotInterface.initializeRobotInterface()
# Service Proxy and Subscriber
self._service_proxy = None
self._subscriber = None
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('client'), 'resource', 'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MyPluginUi')
### Map motors to their respective UI elements
self.motor_widgets = {
0:self._widget.motor0_spinbox,
1:self._widget.motor1_spinbox,
2:self._widget.motor2_spinbox,
3:self._widget.motor3_spinbox,
4:self._widget.motor4_spinbox,
5:self._widget.motor5_spinbox,
6:self._widget.motor6_spinbox,
7:self._widget.motor7_spinbox
}
self.sensor_widgets = {
0:self._widget.sensor0_lineedit,
1:self._widget.sensor1_lineedit,
2:self._widget.sensor2_lineedit,
3:self._widget.sensor3_lineedit,
4:self._widget.sensor4_lineedit,
5:self._widget.sensor5_lineedit,
6:self._widget.sensor6_lineedit,
7:self._widget.sensor7_lineedit,
8:self._widget.sensor8_lineedit,
9:self._widget.sensor9_lineedit,
10:self._widget.sensor10_lineedit,
13:self._widget.sensor13_lineedit,
19:self._widget.sensor19_lineedit,
23:self._widget.sensor23_lineedit,
24:self._widget.sensor24_lineedit,
25:self._widget.sensor25_lineedit,
26:self._widget.sensor26_lineedit,
27:self._widget.sensor27_lineedit,
28:self._widget.sensor28_lineedit,
29:self._widget.sensor29_lineedit,
30:self._widget.sensor30_lineedit,
31:self._widget.sensor31_lineedit,
32:self._widget.sensor32_lineedit,
}
# Assigned zeros for values that indicate motor speed
# Motors 4 and 5 are set to the most recent position from SensorValues
self.zero_values = {
0:0,
1:0,
2:0,
3:0,
6:0,
7:0
}
#print(self.motor_widgets)
# Hook Qt UI Elements up
"""
self._widget.vertical_add_button.pressed.connect(self.increase_linear_speed_pressed)
self._widget.vertical_subtract_button.pressed.connect(self.decrease_linear_speed_pressed)
"""
self.ENABLE_DEBUGGING = False
self._widget.motor0_spinbox.valueChanged.connect(self.motor0_spinbox_changed)
self._widget.motor1_spinbox.valueChanged.connect(self.motor1_spinbox_changed)
self._widget.motor2_spinbox.valueChanged.connect(self.motor2_spinbox_changed)
self._widget.motor3_spinbox.valueChanged.connect(self.motor3_spinbox_changed)
self._widget.motor4_spinbox.valueChanged.connect(self.motor4_spinbox_changed)
self._widget.motor5_spinbox.valueChanged.connect(self.motor5_spinbox_changed)
self._widget.motor6_spinbox.valueChanged.connect(self.motor6_spinbox_changed)
self._widget.motor7_spinbox.valueChanged.connect(self.motor7_spinbox_changed)
self._widget.general_speed_spinbox.valueChanged.connect(self.general_spinbox_changed)
self._widget.general_speed_slider.valueChanged.connect(self.general_slider_changed)
self._widget.emergency_stop_button.pressed.connect(self.estop_pressed)
self._widget.w_button.pressed.connect(self.w_pressed)
self._widget.a_button.pressed.connect(self.a_pressed)
self._widget.s_button.pressed.connect(self.s_pressed)
self._widget.d_button.pressed.connect(self.d_pressed)
self._widget.zero_locomotion_button.pressed.connect(self.zero_locomotion_speeds)
self._widget.start_shift_button.pressed.connect(self.setup_translate_shift_timer)
self._widget.translate_cancel_button.pressed.connect(self.cancel_shift)
self._widget.attitude_shift_button.pressed.connect(self.setup_attitude_timer)
self._widget.attitude_shift_cancel_button.pressed.connect(self.cancel_attitude_shift)
self._widget.update_sensors_button.pressed.connect(self._setup_timer_update_sensors)
self._widget.stop_sensor_update_button.pressed.connect(self._stop_update_timer)
self._widget.debug_checkbox.toggled.connect(self.debugging_checkbox_checked)
# ROS Connection Fields
"""
TODO: Omitted
"""
if self.check_if_published("")
###
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._widget.setFocusPolicy(0x8)
self._widget.setFocus()
### Keyboard teleop setup
self._widget.keyPressEvent = self.keyPressEvent
self._widget.keyReleaseEvent = self.keyReleaseEvent
# timer to consecutively send service messages
self._update_translate_timer = QTimer(self)
self._update_attitude_timer = QTimer(self)
self._update_sensors_timer = QTimer(self)
def debugging_checkbox_checked(self):
debugging_status = self._widget.debug_checkbox.isChecked()
self.ENABLE_DEBUGGING = debugging_status
# Keyboard Teleop with signalling
"""
Messy, but it works, theoretically.
"""
def keyPressEvent(self, event):
motor_speed = self.get_general_motor_val()
#print("general motor value is: %s" % motor_speed)
if event.key() == Qt.Key_W:
#print("W down")
self.w_pressed(motor_speed)
elif event.key() == Qt.Key_S:
#print("S down")
self.s_pressed(motor_speed)
elif event.key() == Qt.Key_A:
#print("A down")
self.a_pressed(motor_speed)
elif event.key() == Qt.Key_D:
#print("D down")
self.d_pressed(motor_speed)
# Emergency stop, triggers for all motors. Can include one explicitly defined for locomotion though
elif event.key() == Qt.Key_E:
print("Emergency Stopping")
self.estop_pressed()
# Deposition keys
elif event.key() == Qt.Key_U:
print("Press U key")
# Arrow keys to manipulate the general spinbox speed
elif event.key() == Qt.Key_Up:
print("Key up")
self._widget.general_speed_spinbox.setValue(motor_speed + 1)
elif event.key() == Qt.Key_Down:
print("Key down")
self._widget.general_speed_spinbox.setValue(motor_speed - 1)
# Generalize sending spinbox value, handle print/error here
def send_spinbox_value(self, motorNum, value):
resp = robotInterface.sendMotorCommand(motorNum, value)
print("For motor %s sent value %s successfully: %s" % (motorNum, value, resp))
# Currently only geared toward locomotion
def keyReleaseEvent(self, event):
if event.key() in (Qt.Key_W, Qt.Key_S, Qt.Key_A, Qt.Key_D):
if not event.isAutoRepeat():
print("Key released")
robotInterface.sendDriveCommand(0, 0)
def set_locomotion_speeds(self, port_speed, starboard_speed):
respPort = robotInterface.sendMotorCommand(0, port_speed)
respStarboard = robotInterface.sendMotorCommand(1, starboard_speed)
print("Set locomotion speeds: %s" % (respPort and respStarboard))
def zero_locomotion_speeds(self):
self.motor_widgets.get(0).setValue(0)
self.motor_widgets.get(1).setValue(0)
self.set_locomotion_speeds(0,0)
def get_general_motor_val(self):
val = int(self._widget.general_speed_spinbox.value())
return val
def w_pressed(self, motor_speed=None):
if motor_speed is None:
motor_speed = self.get_general_motor_val()
robotInterface.sendDriveCommand(0, motor_speed)
if self.ENABLE_DEBUGGING:
print("w key pressed")
def a_pressed(self, motor_speed=None):
if motor_speed is None:
motor_speed = self.get_general_motor_val()
robotInterface.sendDriveCommand(3, motor_speed)
if self.ENABLE_DEBUGGING:
print("a key pressed")
def s_pressed(self, motor_speed=None):
if motor_speed is None:
motor_speed = self.get_general_motor_val()
robotInterface.sendDriveCommand(1, motor_speed)
if self.ENABLE_DEBUGGING:
print("s key pressed")
def d_pressed(self, motor_speed=None):
if motor_speed is None:
motor_speed = self.get_general_motor_val()
robotInterface.sendDriveCommand(2, motor_speed)
if self.ENABLE_DEBUGGING:
print("d key pressed")
def estop_pressed(self):
print("ESTOP: Attempting to stop all motors...")
# Set all known motors to value 0
for motor_id, zero_value in self.zero_values.items():
#ui_widget.setValue(0)
robotInterface.sendMotorCommand(motor_id, 0)
# Motors 4 and 5 are set by position
translationPos = robotInterface.sensorValueMap.get(4)
bcAttitudePos = robotInterface.sensorValueMap.get(5)
print("ESTOP: Setting translation position: %s and attitude: %s" % (translationPos, bcAttitudePos) )
robotInterface.sendMotorCommand(4, translationPos)
robotInterface.sendMotorCommand(5, bcAttitudePos)
# Stop any updated changes to the translation system
self._update_translate_timer = QTimer(self)
# ROS Connection things
"""
@Slot(str)
def _on_topic_changed(self, topic):
topic = str(topic)
self._unregister_publisher()
print("trying topic: ", topic)
if topic == '':
return
try:
self._publisher = rospy.Publisher(topic, Twist, queue_size = 10)
except TypeError:
self._publisher = rospy.Publisher(topic, Twist)
"""
def check_if_published(topic_name):
topic_list = rospy.get_published_topics()
if topic_name in topic_list:
return True
else:
return False
"""
Unregister ROS publisher
"""
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
if self._service_proxy is not None:
# TODO:Doesn't actually shutdown/unregister??
#self._service_proxy.shutdown('Shutting down service proxy...')
self._service_proxy = None
#if robotInterface is not None:
# robotInterface.motorCommandPub.unregister()
#### Speed and Angle change Functions
"""
Individual Motor Change Functions
"""
def motor0_spinbox_changed(self):
val = int(self.motor_widgets.get(0).value())
self.send_spinbox_value(0, val)
def motor1_spinbox_changed(self):
val = int(self.motor_widgets.get(1).value())
self.send_spinbox_value(1, val)
def motor2_spinbox_changed(self):
val = int(self.motor_widgets.get(2).value())
self.send_spinbox_value(2, val)
def motor3_spinbox_changed(self):
val = int(self.motor_widgets.get(3).value())
self.send_spinbox_value(3, val)
def motor4_spinbox_changed(self):
val = int(self.motor_widgets.get(4).value())
self.send_spinbox_value(4, val)
def motor5_spinbox_changed(self):
val = int(self.motor_widgets.get(5).value())
self.send_spinbox_value(5, val)
self._widget.attitude_slider.setValue(val)
def motor5_slider_changed(self):
val = int(self._widget.attitude_slider.value())
### Looky Spinboxes
def motor6_spinbox_changed(self):
val = int(self.motor_widgets.get(6).value())
self.send_spinbox_value(6, val)
def motor7_spinbox_changed(self):
val = int(self.motor_widgets.get(7).value())
self.send_spinbox_value(7, val)
### Translation timer-updated shift to some specified value
def setup_translate_shift_timer(self):
updatesPerSec = int(self._widget.steps_per_sec_spinbox.value())
msUpdate = int(1000/updatesPerSec)
self._update_translate_timer = QTimer()
self._update_translate_timer.timeout.connect(self.shift_timer_func)
self._update_translate_timer.start(msUpdate)
def shift_timer_func(self):
currentValue = self._widget.motor4_spinbox.value()
targetValue = int(self._widget.target_translate_spinbox.value())
if(targetValue == currentValue):
self._update_translate_timer = QTimer(self)
self._widget.translate_cancel_button.setEnabled(False)
return
else:
delta = (targetValue - currentValue)/abs(targetValue - currentValue)
#print(delta)
self._widget.motor4_spinbox.setValue(currentValue + delta)
self._widget.translate_cancel_button.setEnabled(True)
def setup_attitude_timer(self):
updatesPerSec = int(self._widget.steps_attitude_spinbox.value())
msUpdate = int(1000/updatesPerSec)
self._update_attitude_timer = QTimer()
self._update_attitude_timer.timeout.connect(self.shift_timer_attitude_func)
self._update_attitude_timer.start(msUpdate)
def shift_timer_attitude_func(self):
currentValue = self._widget.motor5_spinbox.value()
targetValue = int(self._widget.target_attitude_spinbox.value())
if(targetValue == currentValue):
self._update_attitude_timer = QTimer(self)
self._widget.attitude_shift_cancel_button.setEnabled(False)
else:
delta = (targetValue - currentValue)/abs(targetValue - currentValue)
self._widget.motor5_spinbox.setValue(currentValue + delta)
self._widget.attitude_shift_cancel_button.setEnabled(True)
# Cancel the 'shift' by just setting the reference to a new QTimer
def cancel_shift(self):
self._update_translate_timer.stop()
self._widget.translate_cancel_button.setEnabled(False)
def cancel_attitude_shift(self):
self._update_attitude_timer.stop()
self._widget.attitude_shift_cancel_button.setEnabled(False)
"""
Grouped Motor Control Functions
"""
def general_spinbox_changed(self):
self._widget.general_speed_slider.setValue(self.get_general_motor_val())
if self._widget.general_assign_checkbox.isChecked():
motor_speed = self.get_general_motor_val()
for motor_id, ui_widget in self.motor_widgets.items():
ui_widget.setValue(motor_speed)
else:
return
def general_slider_changed(self):
sliderValue = int(self._widget.general_speed_slider.value())
self._widget.general_speed_spinbox.setValue(sliderValue)
#print("Slider value is: %s" % sliderValue)
"""
Update sensor values
"""
"""
Sending messages
"""
# Iteratively send values
def _on_parameter_changed(self):
for motor_id, ui_widget in self.motor_widgets.items():
# If widget is spinbox,
val = int(ui_widget.value())
resp = self._send_motor_command(motor_id, val)
print("on motor: %s" % motor_id + ", value: %s" % val + "sending result: %s" % resp)
"""
Sends a single commanded value (0-100) to a specified motor id
"""
def _send_motor_command(self, motor_id, val):
try:
robotInterface.sendMotorCommand(motor_id, val)
print("Sent motor command for motor: %s" % motor_id + " with value: %s" % val)
except Exception as exc:
print("There was a problem sending the motor command: " + str(exc))
"""
Sensor value updating
"""
def _stop_update_timer(self):
self._update_sensors_timer = QTimer()
def _setup_timer_update_sensors(self):
updatesPerSec = 100
msUpdate = int(1000/updatesPerSec)
self._update_sensors_timer = QTimer()
self._update_sensors_timer.timeout.connect(self.try_update_sensors)
self._update_sensors_timer.start(msUpdate)
def try_update_sensors(self):
try:
for sensor_id, sensor_widget in self.sensor_widgets.items():
# Try to get value
sensorVal = robotInterface.sensorValueMap.get(sensor_id)
sensor_widget.setText(str(sensorVal))
except Exception as exc:
print("There was an unusual problem updating sensors: " + str(exc))
def _set_status_text(self, text):
self._widget.status_label.setText(text)
def shutdown_plugin(self):
# TODO unregister all publishers here
#self._update_parameter_timer.stop()
self._unregister_publisher()
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
)
except:
print (
"Will attempt to start SpeakEasy in ROS mode. If fail, switch to local mode. Possibly a few seconds delay..."
)
speakeasyController = SpeakEasyController(scriptDir, unix_sig_notify_read_socket=rsock, stand_alone=None)
else:
try:
rospy.loginfo(
"Will attempt to start SpeakEasy in ROS mode. If fail, switch to local mode. Possibly a few seconds delay..."
)
except:
print (
"Will attempt to start SpeakEasy in ROS mode. If fail, switch to local mode. Possibly a few seconds delay..."
)
speakeasyController = SpeakEasyController(scriptDir, unix_sig_notify_read_socket=rsock, stand_alone=None)
# Attach Unix signals USR1/USR2 to the sigusr1_2_handler().
# (These signals are separate from the Qt signals!):
signal.signal(signal.SIGUSR1, sigusr1_2_handler)
signal.signal(signal.SIGUSR2, sigusr1_2_handler)
# Unix signals are delivered to Qt only when Qt
# leaves its event loop. Force that to happen
# every half second:
timer = QTimer()
timer.start(500)
timer.timeout.connect(lambda: None)
# Enter Qt application main loop
sys.exit(app.exec_())
class MobrobDashboardWidget(QWidget):
column_names = ['service', 'type', 'expression']
def __init__(self):
super(MobrobDashboardWidget, self).__init__()
self.setObjectName('MobrobDashboardWidget')
# create context for the expression eval statement
self._eval_locals = {}
for module in (math, random, time):
self._eval_locals.update(module.__dict__)
self._eval_locals['genpy'] = genpy
del self._eval_locals['__name__']
del self._eval_locals['__doc__']
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('mobrob_robin_rqt'), 'resource', 'MobrobDashboard.ui')
loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox})
self._column_index = {}
for column_name in self.column_names:
self._column_index[column_name] = len(self._column_index)
self._twist = Twist()
self.base_activated = False
self._pub = rospy.Publisher('/mobrob_robin/base/drives/control/cmd_vel', Twist, queue_size = 1)
self._timer_refresh_state = QTimer(self)
self._timer_refresh_state.timeout.connect(self._timer_update)
self.start()
def start(self):
self._timer_refresh_state.start(10)
#def save_settings(self, plugin_settings, instance_settings):
def shutdown_plugin(self):
self._timer_refresh_state.stop()
self._sub_lwa_state.unregister()
self._sub_gripper_state.unregister()
self._sub_pan_tilt_state.unregister()
#def restore_settings(self, plugin_settings, instance_settings):
#def trigger_configuration(self):
def _timer_update(self):
self._publish_twist()
self._update_goals()
def _publish_twist(self):
if self.base_activated:
self._pub.publish(self._twist)
@Slot()
def refresh_state(self):
self.test_label.setText('test')
def call_service( self, string ):
service_name = string
service_class = rosservice.get_service_class_by_name( service_name )
service = rospy.ServiceProxy(service_name, service_class)
request = service_class._request_class()
try:
response = service()
except rospy.ServiceException as e:
qWarning('service_caller: request:\n%r' % (request))
qWarning('service_caller: error calling service "%s":\n%s' % (self._service_info['service_name'], e))
#else:
#print(response)
##### Base
def _base_move(self, x, y, yaw):
twist = Twist()
twist.linear.x = x;
twist.linear.y = y;
twist.angular.x = 0.0;
twist.angular.y = 0.0;
twist.angular.z = yaw;
self._twist = twist
def _base_stop_motion(self):
self.base_activated = False
self._base_move(0.0, 0.0, 0.0)
self._pub.publish(self._twist)
@Slot()
def on_base_reference_clicked(self):
self.call_service( '/mobrob_robin/base/drives/control/reference' )
@Slot()
def on_base_start_clicked(self):
self.call_service( '/mobrob_robin/base/drives/control/start' )
@Slot()
def on_base_stop_clicked(self):
self.call_service( '/mobrob_robin/base/drives/control/stop' )
@Slot()
def on_base_forward_pressed(self):
self._base_move(0.3, 0.0, 0.0)
self.base_activated = True
@Slot()
def on_base_forward_released(self):
self._base_stop_motion()
@Slot()
def on_base_back_pressed(self):
self._base_move(-0.3, 0.0, 0.0)
self.base_activated = True
@Slot()
def on_base_back_released(self):
self._base_stop_motion()
@Slot()
def on_base_left_pressed(self):
self._base_move(0.0, 0.3, 0.0)
self.base_activated = True
@Slot()
def on_base_left_released(self):
self._base_stop_motion()
@Slot()
def on_base_right_pressed(self):
self._base_move(0.0, -0.3, 0.0)
self.base_activated = True
@Slot()
def on_base_right_released(self):
self._base_stop_motion()
@Slot()
def on_base_turn_left_pressed(self):
self._base_move(0.0, 0.0, 0.5)
self.base_activated = True
@Slot()
def on_base_turn_left_released(self):
self._base_stop_motion()
@Slot()
def on_base_turn_right_pressed(self):
self._base_move(0.0, 0.0, -0.5)
self.base_activated = True
@Slot()
def on_base_turn_right_released(self):
self._base_stop_motion()
#diagonal
@Slot()
def on_base_forward_left_pressed(self):
self._base_move(0.2, 0.2, 0.0)
self.base_activated = True
@Slot()
def on_base_forward_left_released(self):
self._base_stop_motion()
@Slot()
def on_base_forward_right_pressed(self):
self._base_move(0.2, -0.2, 0.0)
self.base_activated = True
@Slot()
def on_base_forward_right_released(self):
self._base_stop_motion()
@Slot()
def on_base_back_left_pressed(self):
self._base_move(-0.2, 0.2, 0.0)
self.base_activated = True
@Slot()
def on_base_back_left_released(self):
self._base_stop_motion()
@Slot()
def on_base_back_right_pressed(self):
self._base_move(-0.2, -0.2, 0.0)
self.base_activated = True
@Slot()
def on_base_back_right_released(self):
self._base_stop_motion()
class Top(Plugin):
NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads']
OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ]
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ]
SORT_TYPE = [str, str, float, float, float ]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# if not args.quiet:
# print 'arguments: ', args
# print 'unknowns: ', unknowns
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.iteritems():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex', int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class View(QWidget):
def __init__(self, parent=None):
super(View, self).__init__()
self.unit = 'deg'
self.timer = QTimer(self)
self.timer.timeout.connect(lambda: self.publishRequested.emit())
pkg_path = rospkg.RosPack().get_path('rqt_tf_rot')
ui_file_path = os.path.join(pkg_path, 'res', 'PluginUI.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file_path, self)
self.initUI()
publishRequested = Signal()
def initUI(self):
btnPublish = self.findChild(QAbstractButton, 'btnPublish')
btnPublish.clicked.connect(lambda: self.publishRequested.emit())
self.txtChild = self.findChild(QLineEdit, 'txtChild')
self.cboParent = self.findChild(QComboBox, 'cboParent')
self.spnRollRad = self.findChild(QDoubleSpinBox, 'spnRollRad')
self.spnPitchRad = self.findChild(QDoubleSpinBox, 'spnPitchRad')
self.spnYawRad = self.findChild(QDoubleSpinBox, 'spnYawRad')
spnTimeout = self.findChild(QSpinBox, 'spnTimeout')
spnTimeout.valueChanged.connect(
lambda timeout: self.timer.setInterval(timeout))
chkTimer = self.findChild(QCheckBox, 'chkTimer')
chkTimer.stateChanged.connect(self.onChkTimerChanged)
for suffix in ['Yaw', 'Pitch', 'Roll']:
degSpin = self.findChild(QDoubleSpinBox, 'spn'+suffix+'Deg')
radSpin = self.findChild(QDoubleSpinBox, 'spn'+suffix+'Rad')
dial = self.findChild(QDial, 'dial'+suffix)
# May the Python gods forgive me
# The lambda default parameters are there only to simulate
# what would happen if lambdas remembered what objects
# they were trying to capture instead of just the names of
# the variables in the closure
dial.valueChanged.connect(
lambda value, degSpin=degSpin, radSpin=radSpin:
self.onDialChanged(value,
deg_spinbox=degSpin,
rad_spinbox=radSpin))
degSpin.valueChanged.connect(
lambda value, dial=dial, radSpin=radSpin:
self.onDegSpinChanged(
value,
slider=dial,
rad_spinbox=radSpin))
radSpin.valueChanged.connect(
lambda value, dial=dial, degSpin=degSpin:
self.onRadSpinChanged(value,
slider=dial,
deg_spinbox=degSpin))
# --- Slots (not all strictly Qt slots)
def onDialChanged(self, value, deg_spinbox=None, rad_spinbox=None):
setValueWithoutSignals(deg_spinbox, float(value))
setValueWithoutSignals(rad_spinbox,
convertUnit(float(value), 'deg', 'rad'))
def onDegSpinChanged(self, value, slider=None, rad_spinbox=None):
setValueWithoutSignals(slider, int(value))
setValueWithoutSignals(rad_spinbox,
convertUnit(float(value), 'deg', 'rad'))
def onRadSpinChanged(self, value, slider=None, deg_spinbox=None):
setValueWithoutSignals(slider, int(value))
setValueWithoutSignals(deg_spinbox,
convertUnit(float(value), 'rad', 'deg'))
def onChkTimerChanged(self, state):
if state == Qt.Checked:
self.timer.start()
else:
self.timer.stop()
@Slot(str)
def addLinkToList(self, child_id):
if self.cboParent != None:
self.cboParent.addItem(child_id)
# --- Accessors
def childName(self):
if self.txtChild != None:
return self.txtChild.text()
else:
return ''
def parentName(self):
if self.cboParent != None:
return self.cboParent.currentText()
else:
return ''
def roll(self):
return self.spnRollRad.value()
def pitch(self):
return self.spnPitchRad.value()
def yaw(self):
return self.spnYawRad.value()
class SysCheck(Plugin):
def __init__(self, context):
super(SysCheck, self).__init__(context)
# Set the name of the object
# (Usually should be the same as the class name)
self.setObjectName('SysCheck')
# Get the thruster parameters
self.names = []
try:
self.names = rospy.get_param('thrusters/mapping')
except KeyError:
print "Thruster mapping not loaded into parameter server"
# Setup the publisher and message object for sending thruster messages
self.pub = rospy.Publisher('thruster', thruster, queue_size=1)
self.thrusterMessage = thruster()
# Subscribe to the depth and orientation topics
self.depth_sub = rospy.Subscriber('depth', Float32Stamped,
self.depthSubCallback, queue_size=1)
self.imu_sub = rospy.Subscriber('pretty/orientation',
Euler,
self.imuSubCallback, queue_size=1)
# Initialize the timers
self.depthTimer = QTimer(self)
self.imuTimer = QTimer(self)
self.sendTimer = QTimer(self)
self.depthTimer.timeout.connect(self.depthMissed)
self.imuTimer.timeout.connect(self.imuMissed)
self.sendTimer.timeout.connect(self.sendMessage)
self.depthTimer.start(1000)
self.imuTimer.start(1000)
# Only start the param timer if the params aren't loaded
if len(self.names) == 0:
self.paramTimer = QTimer(self)
self.paramTimer.timeout.connect(self.loadParam)
self.paramTimer.start(1000)
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of
# this package
ui_file = os.path.join(rospkg.RosPack().get_path('robosub'),
'src/rqt/rqt_syscheck/resource', 'SysCheck.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects a name (Usually the class name + 'Ui')
self._widget.setObjectName('SysCheckUi')
# Add RoboSub Logo to the GUI
logo_file = os.path.join(rospkg.RosPack().get_path('robosub'),
'src/rqt/resource', 'robosub_logo.png')
self._widget.setStyleSheet(".QWidget {background-image: url(" +
logo_file +
"); background-repeat: no-repeat;" +
"background-position:bottom right}")
# Hide the stale labels on init
self._widget.imuStale.hide()
self._widget.depthStale.hide()
# Connect the valueChanged signal to our updateSpeed function
self._widget.thrusterSpeed.valueChanged[int].connect(self.updateSpeed)
self._widget.thrusterEnable.setCheckable(True)
self._widget.thrusterEnable.toggled[bool].connect(self.enable)
self._widget.thrusterKill.clicked[bool].connect(self.kill)
# Load in the thruster buttons and connect callbacks
self.thrusterButtons = []
self.thrusterScales = []
self.thrusterCallbacks = {}
self.loadThrusters()
# If the context is not the root add the serial number to the window
# title
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)
def kill(self):
for i in self.thrusterButtons:
i.setChecked(False)
self._widget.thrusterSpeed.setValue(0)
def enable(self, s):
if s:
self.sendTimer.start(1000)
else:
self.sendTimer.stop()
def loadThrusters(self):
# Loop over all of the thruster values found in the params
for i in range(0, len(self.names)):
# Add a button to a list so we can mess with it later
self.thrusterButtons.append(QPushButton(self.names[i]['name']))
# Modify setting of the button
self.thrusterButtons[i].setCheckable(True)
# Save the callbacks in a list
self.thrusterCallbacks[self.names[i]['name']] = \
getattr(self, '_handle_thruster' + str(i))
# Connect the callback to the button's toggle event
self.thrusterButtons[i].toggled[bool].connect(
self.thrusterCallbacks[self.names[i]['name']])
# Add the button to the Ui
self._widget.thrusterButtons.addWidget(self.thrusterButtons[i])
# Get the orientation
self.thrusterScales.append(0)
for v in self.names[i]['orientation'].values():
self.thrusterScales[i] = self.thrusterScales[i] + v
# Append a value to the thruster message for this button
self.thrusterMessage.data.append(0.0)
print self.thrusterScales
def loadParam(self):
try:
self.names = rospy.get_param('thrusters/mapping')
self.loadThrusters()
# Stop the timer if the params were successfully loaded
self.paramTimer.stop()
except KeyError:
# Don't throw an error if we hit a KeyError
pass
def shutdown_plugin(self):
# Stop the send timer before unregistering the publisher
self.sendTimer.stop()
# Unregister the thruster publisher and subscribers
self.pub.unregister()
self.depth_sub.unregister()
self.imu_sub.unregister()
# Stop the Other Timers
try:
self.paramTimer.stop()
except AttributeError:
pass
self.imuTimer.stop()
self.depthTimer.stop()
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 imuMissed(self):
# If an Imu message was not received by the time the timer fired
# show the stale label and hide the active label
if not self._widget.imuStale.isVisible():
self._widget.imuStale.show()
if self._widget.imuActive.isVisible():
self._widget.imuActive.hide()
def imuSubCallback(self, m):
# Stop the imuTimer so it doesn't fire in this function
self.imuTimer.stop()
# Reset the active label hiding the stale label
if self._widget.imuStale.isVisible():
self._widget.imuStale.hide()
if not self._widget.imuActive.isVisible():
self._widget.imuActive.show()
self._widget.currentRoll.setText(str(m.roll))
self._widget.currentPitch.setText(str(m.pitch))
self._widget.currentYaw.setText(str(m.yaw))
# Restart the timer
self.imuTimer.start(1000)
def depthMissed(self):
# If an Depth message was not received by the time the timer fired
# show the stale label and hide the active label
if not self._widget.depthStale.isVisible():
self._widget.depthStale.show()
if self._widget.depthActive.isVisible():
self._widget.depthActive.hide()
def depthSubCallback(self, m):
# Stop the imuTimer so it doesn't fire in this function
self.depthTimer.stop()
# Reset the active label hiding the stale label
if self._widget.depthStale.isVisible():
self._widget.depthStale.hide()
if not self._widget.depthActive.isVisible():
self._widget.depthActive.show()
self._widget.currentDepth.setText("{0:.2f}".format(m.data))
# Restart the timer
self.depthTimer.start(1000)
def sendMessage(self):
# Publish the message that we have constructed
self.pub.publish(self.thrusterMessage)
# Update the speeds in the thruster message based on the slider
def updateSpeed(self, value):
# Update the speed label so the user knows the value that is set
self._widget.speedLabel.setText("Speed ({:+.2f})".format(
float(value)/100))
# Loop over the thruster message and update the value
for i in range(0, len(self.thrusterMessage.data)):
# Check if the thruster is enabled
if self.thrusterButtons[i].isChecked():
self.thrusterMessage.data[i] = self.thrusterScales[i] * \
float(value)/100
'''
The following functions handle updating the thruster message based on
the buttons.
'''
def _handle_thruster0(self, state):
if state:
self.thrusterMessage.data[0] = 0.01 * self.thrusterScales[0] * \
self._widget.thrusterSpeed.value()
else:
self.thrusterMessage.data[0] = 0
def _handle_thruster1(self, state):
if state:
self.thrusterMessage.data[1] = 0.01 * self.thrusterScales[1] * \
self._widget.thrusterSpeed.value()
else:
self.thrusterMessage.data[1] = 0
def _handle_thruster2(self, state):
if state:
self.thrusterMessage.data[2] = 0.01 * self.thrusterScales[2] * \
self._widget.thrusterSpeed.value()
else:
self.thrusterMessage.data[2] = 0
def _handle_thruster3(self, state):
if state:
self.thrusterMessage.data[3] = 0.01 * self.thrusterScales[3] * \
self._widget.thrusterSpeed.value()
else:
self.thrusterMessage.data[3] = 0
def _handle_thruster4(self, state):
if state:
self.thrusterMessage.data[4] = 0.01 * self.thrusterScales[4] * \
self._widget.thrusterSpeed.value()
else:
self.thrusterMessage.data[4] = 0
def _handle_thruster5(self, state):
if state:
self.thrusterMessage.data[5] = 0.01 * self.thrusterScales[5] * \
self._widget.thrusterSpeed.value()
else:
self.thrusterMessage.data[5] = 0
def _handle_thruster6(self, state):
if state:
self.thrusterMessage.data[6] = 0.01 * self.thrusterScales[6] * \
self._widget.thrusterSpeed.value()
else:
self.thrusterMessage.data[6] = 0
def _handle_thruster7(self, state):
if state:
self.thrusterMessage.data[7] = 0.01 * self.thrusterScales[7] * \
self._widget.thrusterSpeed.value()
else:
self.thrusterMessage.data[7] = 0
class ThrusterPlugin(Plugin):
def __init__(self, context):
super(ThrusterPlugin, self).__init__(context)
self.setObjectName('ThrusterPlugin')
self._listener = ThrusterListener()
self._thruster_widgets = {}
self._manual_control = False
self._widget = QWidget()
loadUi(os.path.join(uipath, 'thrusterplugin.ui'), self._widget)
context.add_widget(self._widget)
self._widget.findChild(QPushButton, 'stopAllButton').clicked.connect(self._on_stopall_clicked)
self._widget.findChild(QCheckBox, 'manualControlCheckBox').clicked.connect(self._on_manualcontrol_clicked)
self._update_timer = QTimer(self._widget)
self._update_timer.timeout.connect(self._update)
self._update_timer.start(100)
def _update(self):
active_ids = set()
for thruster_info in self._listener.get_thrusters():
active_ids.add(thruster_info.id)
if thruster_info.id not in self._thruster_widgets:
self._add_thruster_widget(thruster_info.id)
for id in self._thruster_widgets.keys():
if id not in active_ids:
self._remove_thruster_widget(id)
else:
self._update_thruster(id)
def _add_thruster_widget(self, id):
self._widget.findChild(QLabel, 'noThrustersLabel').setVisible(False)
thruster_widget = QWidget()
thruster_frame = self._widget.findChild(QFrame, 'thrusterFrame')
pos = sum(1 for existing_id in self._thruster_widgets if id > existing_id)
thruster_frame.layout().insertWidget(pos, thruster_widget)
loadUi(os.path.join(uipath, 'thruster.ui'), thruster_widget)
thruster_widget.findChild(QLabel, 'thrusterLabel').setText(id)
thruster_widget.findChild(QPushButton, 'offButton').clicked.connect(lambda: self._on_stop_clicked(id))
self._thruster_widgets[id] = thruster_widget
return thruster_widget
def _remove_thruster_widget(self, thruster):
if thruster not in self._thruster_widgets:
return
self._thruster_widgets[thruster].deleteLater()
del self._thruster_widgets[thruster]
if len(self._thruster_widgets) == 0:
self._widget.findChild(QLabel, 'noThrustersLabel').setVisible(True)
def _update_thrusters(self):
for thruster in self._listener.get_thrusters():
self._update_thruster(thruster)
def _update_thruster(self, id):
thruster = self._listener.get_thruster(id)
thruster_widget = self._thruster_widgets[id]
slider_widget = thruster_widget.findChild(QSlider, 'thrusterSlider')
reverse_widget = thruster_widget.findChild(QCheckBox, 'reverseCheckbox')
thruster_widget.findChild(QPushButton, 'offButton').setEnabled(self._manual_control)
thruster_widget.findChild(QCheckBox, 'reverseCheckbox').setEnabled(self._manual_control)
thruster_widget.findChild(QSlider, 'thrusterSlider').setEnabled(self._manual_control)
if not self._manual_control:
command = self._listener.get_command(id)
if command is None:
return
if command.force > 0:
effort = command.force / thruster.max_force
else:
effort = command.force / thruster.min_force
slider_widget.setValue(abs(effort) * 100)
reverse_widget.setChecked(effort < 0)
else:
effort = slider_widget.value()/100
if not reverse_widget.isChecked():
force = effort * thruster.max_force
else:
force = effort * thruster.min_force
self._listener.send_command(id, force)
if abs(effort) > .9:
color = 'red'
elif abs(effort) > .75:
color = 'yellow'
else:
color = 'black'
thruster_widget.findChild(QLabel, 'thrusterLabel').setText(
'<span style="color: ' + color + '">' + id + '</span>')
def _on_stopall_clicked(self):
for thruster_widget in self._thruster_widgets.values():
thruster_widget.findChild(QSlider, 'thrusterSlider').setValue(0)
def _on_stop_clicked(self, id):
self._thruster_widgets[id].findChild(QSlider, 'thrusterSlider').setValue(0)
def _on_manualcontrol_clicked(self):
self._manual_control = self._widget.findChild(QCheckBox, 'manualControlCheckBox').isChecked()
def shutdown_plugin(self):
self._listener.unregister()
class PluginLogManager(Plugin):
"""
rqt_console plugin's main class. Handles communication with ros_gui and contains
callbacks to handle incoming message
"""
def __init__(self, context):
Plugin.__init__(self, context)
def onCreate(self, param):
layout = QGridLayout(self)
layout.setContentsMargins(2, 2, 2, 2)
self._rospack = rospkg.RosPack()
self._model = MessageDataModel()
self._proxy_model = MessageProxyModel()
self._proxy_model.setSourceModel(self._model)
self.__widget = ConsoleWidget(self._proxy_model, self._rospack)
layout.addWidget(self.__widget, 0, 0, 0, 0)
# queue to store incoming data which get flushed periodically to the model
# required since QSortProxyModel can not handle a high insert rate
self._message_queue = []
self._mutex = QMutex()
self._timer = QTimer()
self._timer.timeout.connect(self.insert_messages)
self._timer.start(100)
self._subscriber = None
self._topic = '/rosout_agg'
self._subscribe(self._topic)
def queue_message(self, log_msg):
"""
Callback for adding an incomming message to the queue.
"""
if not self.__widget._paused:
msg = PluginLogManager.convert_rosgraph_log_message(log_msg)
with QMutexLocker(self._mutex):
self._message_queue.append(msg)
@staticmethod
def convert_rosgraph_log_message(log_msg):
msg = Message()
msg.set_stamp_format('hh:mm:ss.ZZZ (yyyy-MM-dd)')
msg.message = log_msg.msg
msg.severity = log_msg.level
msg.node = log_msg.name
msg.stamp = (log_msg.header.stamp.secs, log_msg.header.stamp.nsecs)
msg.topics = sorted(log_msg.topics)
msg.location = log_msg.file + ':' + log_msg.function + ':' + str(
log_msg.line)
return msg
def insert_messages(self):
"""
Callback for flushing incoming Log messages from the queue to the model.
"""
with QMutexLocker(self._mutex):
msgs = self._message_queue
self._message_queue = []
if msgs:
self._model.insert_rows(msgs)
def shutdown_plugin(self):
self._subscriber.unregister()
self._timer.stop()
self.__widget.cleanup_browsers_on_close()
def save_settings(self, plugin_settings, instance_settings):
self.__widget.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self.__widget.restore_settings(plugin_settings, instance_settings)
def trigger_configuration(self):
topics = [
t for t in rospy.get_published_topics()
if t[1] == 'rosgraph_msgs/Log'
]
topics.sort(key=lambda tup: tup[0])
dialog = ConsoleSettingsDialog(topics, self._rospack)
(topic, message_limit) = dialog.query(self._topic,
self._model.get_message_limit())
if topic != self._topic:
self._subscribe(topic)
if message_limit != self._model.get_message_limit():
self._model.set_message_limit(message_limit)
def _subscribe(self, topic):
if self._subscriber:
self._subscriber.unregister()
self._subscriber = rospy.Subscriber(topic, Log, self.queue_message)
self._currenttopic = topic
def onStart(self):
pass
def onPause(self):
pass
def onResume(self):
pass
def onControlModeChanged(self, mode):
if mode == ControlMode.AUTOMATIC:
self.setEnabled(False)
else:
self.setEnabled(True)
def onUserChanged(self, user_info):
pass
def onTranslate(self, lng):
pass
def onEmergencyStop(self, state):
pass
def onDestroy(self):
self.shutdown_plugin()
class RuntimeMonitorWidget(QWidget):
def __init__(self, topic="diagnostics"):
super(RuntimeMonitorWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_runtime_monitor'), 'resource', 'runtime_monitor_widget.ui')
loadUi(ui_file, self)
self.setObjectName('RuntimeMonitorWidget')
self._mutex = threading.Lock()
self._error_icon = QIcon.fromTheme('dialog-error')
self._warning_icon = QIcon.fromTheme('dialog-warning')
self._ok_icon = QIcon.fromTheme('dialog-information')
self._stale_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Stale (0)'])
self._stale_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._stale_node)
self._error_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Errors (0)'])
self._error_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._error_node)
self._warning_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Warnings (0)'])
self._warning_node.setIcon(0, self._warning_icon)
self.tree_widget.addTopLevelItem(self._warning_node)
self._ok_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Ok (0)'])
self._ok_node.setIcon(0, self._ok_icon)
self.tree_widget.addTopLevelItem(self._ok_node)
self.tree_widget.itemSelectionChanged.connect(self._refresh_selection)
self.keyPressEvent = self._on_key_press
self._name_to_item = {}
self._new_errors_callback = None
self._subscriber = rospy.Subscriber(topic, DiagnosticArray, self._diagnostics_callback)
self._previous_ros_time = rospy.Time.now()
self._timer = QTimer()
self._timer.timeout.connect(self._on_timer)
self._timer.start(1000)
self._msg_timer = QTimer()
self._msg_timer.timeout.connect(self._update_messages)
self._msg_timer.start(100)
self._messages = []
self._used_items = 0
def __del__(self):
self.shutdown()
def shutdown(self):
"""
Unregisters subscriber and stops timers
"""
self._msg_timer.stop()
self._timer.stop()
if rospy.is_shutdown():
return
if self._subscriber:
self._subscriber.unregister()
self._subscriber = None
def change_diagnostic_topic(self, topic):
"""
Changes diagnostics topic name. Must be of type diagnostic_msgs/DiagnosticArray
"""
if not topic:
self.reset_monitor()
return
if self._subscriber:
self._subscriber.unregister()
self._subscriber = rospy.Subscriber(str(topic), DiagnosticArray, self._diagnostics_callback)
self.reset_monitor()
def reset_monitor(self):
"""
Removes all values from monitor display, resets buffers
"""
self._name_to_item = {} # Reset all stale topics
self._messages = []
self._clear_tree()
def _clear_tree(self):
for index in range(self._stale_node.childCount()):
self._stale_node.removeChild(self._stale_node.child(index))
for index in range(self._error_node.childCount()):
self._error_node.removeChild(self._error_node.child(index))
for index in range(self._warning_node.childCount()):
self._warning_node.removeChild(self._warning_node.child(index))
for index in range(self._ok_node.childCount()):
self._ok_node.removeChild(self._ok_node.child(index))
self._update_root_labels()
# Diagnostics callbacks (subscriber thread)
def _diagnostics_callback(self, message):
with self._mutex:
self._messages.append(message)
# Update display of messages from main thread
def _update_messages(self):
with self._mutex:
messages = self._messages[:]
self._messages = []
had_errors = False
for message in messages:
for status in message.status:
was_selected = False
if (self._name_to_item.has_key(status.name)):
item = self._name_to_item[status.name]
if item.tree_node.isSelected():
was_selected = True
if (item.status.level == DiagnosticStatus.ERROR and status.level != DiagnosticStatus.ERROR):
had_errors = True
self._update_item(item, status, was_selected)
else:
self._create_item(status, was_selected, True)
if (status.level == DiagnosticStatus.ERROR):
had_errors = True
if (had_errors and self._new_errors_callback != None):
self._new_errors_callback()
self._update_root_labels()
self.update()
self._refresh_selection()
def _update_item(self, item, status, was_selected):
change_parent = False
if (item.status.level != status.level):
change_parent = True
if (change_parent):
if (item.status.level == DiagnosticStatus.OK):
self._ok_node.removeChild(item.tree_node)
elif (item.status.level == DiagnosticStatus.WARN):
self._warning_node.removeChild(item.tree_node)
elif (item.status.level == -1) or (item.status.level == DiagnosticStatus.STALE):
self._stale_node.removeChild(item.tree_node)
else: # ERROR
self._error_node.removeChild(item.tree_node)
if (status.level == DiagnosticStatus.OK):
parent_node = self._ok_node
elif (status.level == DiagnosticStatus.WARN):
parent_node = self._warning_node
elif (status.level == -1) or (status.level == DiagnosticStatus.STALE):
parent_node = self._stale_node
else: # ERROR
parent_node = self._error_node
item.tree_node.setText(0, status.name + ": " + status.message)
item.tree_node.setData(0, Qt.UserRole, item)
parent_node.addChild(item.tree_node)
# expand errors automatically
if (status.level > 1 or status.level == -1):
parent_node.setExpanded(True)
parent_node.sortChildren(0, Qt.AscendingOrder)
if (was_selected):
self.tree_widget.setCurrentItem(item.tree_node)
else:
item.tree_node.setText(0, status.name + ": " + status.message)
item.status = status
if (was_selected):
self._fillout_info(item.tree_node)
item.mark = True
def _create_item(self, status, select, expand_if_error):
if (status.level == DiagnosticStatus.OK):
parent_node = self._ok_node
elif (status.level == DiagnosticStatus.WARN):
parent_node = self._warning_node
elif (status.level == -1) or (status.level == DiagnosticStatus.STALE):
parent_node = self._stale_node
else: # ERROR
parent_node = self._error_node
item = TreeItem(status, QTreeWidgetItem(parent_node, [status.name + ": " + status.message]))
item.tree_node.setData(0, Qt.UserRole, item)
parent_node.addChild(item.tree_node)
self._name_to_item[status.name] = item
parent_node.sortChildren(0, Qt.AscendingOrder)
if (select):
item.tree_node.setSelected(True)
if (expand_if_error and (status.level > 1 or status.level == -1)):
parent_node.setExpanded(True)
item.mark = True
return item
def _fillout_info(self, node):
item = node.data(0, Qt.UserRole)
if not item:
return
scroll_value = self.html_browser.verticalScrollBar().value()
status = item.status
s = cStringIO.StringIO()
s.write("<html><body>")
s.write("<b>Component</b>: %s<br>\n" % (status.name))
s.write("<b>Message</b>: %s<br>\n" % (status.message))
s.write("<b>Hardware ID</b>: %s<br><br>\n\n" % (status.hardware_id))
s.write('<table border="1" cellpadding="2" cellspacing="0">')
for value in status.values:
value.value = value.value.replace("\n", "<br>")
s.write("<tr><td><b>%s</b></td> <td>%s</td></tr>\n" % (value.key, value.value))
s.write("</table></body></html>")
self.html_browser.setHtml(s.getvalue())
if self.html_browser.verticalScrollBar().maximum() < scroll_value:
scroll_value = self.html_browser.verticalScrollBar().maximum()
self.html_browser.verticalScrollBar().setValue(scroll_value)
def _refresh_selection(self):
current_item = self.tree_widget.selectedItems()
if current_item:
self._fillout_info(current_item[0])
def _on_key_press(self, event):
key = event.key()
if key == Qt.Key_Delete:
nodes = self.tree_widget.selectedItems()
if (nodes != [] and nodes[0] not in (self._ok_node, self._warning_node, self._stale_node, self._error_node)):
item = nodes[0].data(0, Qt.UserRole)
if (item.status.level == 0):
self._ok_node.removeChild(item.tree_node)
elif (item.status.level == 1):
self._warning_node.removeChild(item.tree_node)
elif (item.status.level == -1) or (item.status.level == DiagnosticStatus.STALE):
self._stale_node.removeChild(item.tree_node)
else:
self._error_node.removeChild(item.tree_node)
del self._name_to_item[item.status.name]
self._update_root_labels()
self.update()
event.accept()
else:
event.ignore()
def _on_timer(self):
if self._previous_ros_time + rospy.Duration(5) > rospy.Time.now():
return
self._previous_ros_time = rospy.Time.now()
for name, item in self._name_to_item.iteritems():
node = item.tree_node
if (item != None):
if (not item.mark):
was_selected = False
selected = self.tree_widget.selectedItems()
if selected != [] and selected[0] == node:
was_selected = True
new_status = copy.deepcopy(item.status)
new_status.level = -1 # mark stale
self._update_item(item, new_status, was_selected)
item.mark = False
self._update_root_labels()
self.update()
def set_new_errors_callback(self, callback):
self._new_errors_callback = callback
def _update_root_labels(self):
self._stale_node.setText(0, "Stale (%s)" % (self._stale_node.childCount()))
self._error_node.setText(0, "Errors (%s)" % (self._error_node.childCount()))
self._warning_node.setText(0, "Warnings (%s)" % (self._warning_node.childCount()))
self._ok_node.setText(0, "Ok (%s)" % (self._ok_node.childCount()))
class TFBroadcasterWidget(QWidget):
def __init__(self, widget):
super(TFBroadcasterWidget, self).__init__()
rospkg_pack = rospkg.RosPack()
ui_file = os.path.join(rospkg_pack.get_path('my_common'), 'resource', 'TFBroadcaster.ui')
loadUi(ui_file, self)
self._frame_id, self._child_frame_id = FRAME_ID, CHILD_FRAME_ID
self._tf_manager = TFBroadcasterImpl(self._frame_id, self._child_frame_id)
self.setup_ui()
self._updateTimer = QTimer(self)
self._updateTimer.timeout.connect(self.timeout_callback)
def setup_ui(self):
self._status_widget = StatusInputWidgetSet(self.qt_stop_radiobtn,
self.qt_static_tf_radiobtn,
self.qt_tf_radiobtn,
self._tf_manager.set_stop_status,
self._tf_manager.set_static_tf_status,
self._tf_manager.set_tf_status)
self._frame_id_widget = FrameIDInputWidgetSet(self.qt_frame_id_lineedit,
self.qt_frame_id_update_btn,
self._frame_id,
self._tf_manager.set_frame_id)
self._child_frame_id_widget = FrameIDInputWidgetSet(self.qt_child_frame_id_lineedit,
self.qt_child_frame_id_update_btn,
self._child_frame_id,
self._tf_manager.set_child_frame_id)
self._x_widget = SliderSpinboxInputWidgetSet(self.qt_x_slider,
self.qt_x_spinbox,
self.qt_x_btn,
0.0, POS_MAX_VALUE, POS_MIN_VALUE)
self._y_widget = SliderSpinboxInputWidgetSet(self.qt_y_slider,
self.qt_y_spinbox,
self.qt_y_btn,
0.0, POS_MAX_VALUE, POS_MIN_VALUE)
self._z_widget = SliderSpinboxInputWidgetSet(self.qt_z_slider,
self.qt_z_spinbox,
self.qt_z_btn,
0.0, POS_MAX_VALUE, POS_MIN_VALUE)
self._roll_widget = SliderSpinboxInputWidgetSet(self.qt_roll_slider,
self.qt_roll_spinbox,
self.qt_roll_btn,
0.0, ORI_MAX_VALUE, ORI_MIN_VALUE)
self._pitch_widget = SliderSpinboxInputWidgetSet(self.qt_pitch_slider,
self.qt_pitch_spinbox,
self.qt_pitch_btn,
0.0, ORI_MAX_VALUE, ORI_MIN_VALUE)
self._yaw_widget = SliderSpinboxInputWidgetSet(self.qt_yaw_slider,
self.qt_yaw_spinbox,
self.qt_yaw_btn,
0.0, ORI_MAX_VALUE, ORI_MIN_VALUE)
def start(self):
self._tf_manager.set_frames(self._frame_id, self._child_frame_id)
self._updateTimer.start(10) # loop rate[ms]
def stop(self):
self._updateTimer.stop()
def timeout_callback(self):
self._tf_manager.set_position(self._x_widget.value, self._y_widget.value, self._z_widget.value)
self._tf_manager.set_orientation(self._roll_widget.value,
self._pitch_widget.value,
self._yaw_widget.value,
is_rad=False)
self._tf_manager.broadcast_tf()
# override
def save_settings(self, plugin_settings, instance_settings):
self._frame_id = self._frame_id_widget.str
self._child_frame_id = self._child_frame_id_widget.str
instance_settings.set_value('frame_ids', (self._frame_id, self._child_frame_id))
# override
def restore_settings(self, plugin_settings, instance_settings):
frames = instance_settings.value('frame_ids')
try:
self._frame_id, self._child_frame_id = frames
self._frame_id_widget.update(self._frame_id)
self._child_frame_id_widget.update(self._child_frame_id)
except Exception:
self._frame_id, self._child_frame_id = FRAME_ID, CHILD_FRAME_ID
# override
def shutdown_plugin(self):
self.stop()
class ThetaViewWidget(QWidget):
def __init__(self, plugin):
super(ThetaViewWidget, self).__init__()
rp = rospkg.RosPack()
loadUi(
os.path.join(rp.get_path('rqt_theta_viewer'), 'resource',
'ThetaViewerWidget.ui'), self)
self.plugin = plugin
self.events = ThetaViewEvents(self)
self._pushButton_open.clicked.connect(self.events.open_FileDialog)
self._pushButton_save.clicked.connect(self.events.save_FileDialog)
self._pushButton_shutter.clicked.connect(
self.events.shutter_clicked_event)
self.initialize_vals()
self.initialize_glview()
self.initialize_timer()
# ==============================================
# rqt requires
def save_settings(self, plugin_settings, instance_settings):
view_matrix_string = repr(self._glview.get_view_matrix())
instance_settings.set_value('view_matrix', view_matrix_string)
def restore_settings(self, plugin_settings, instance_settings):
view_matrix_string = instance_settings.value('view_matrix')
try:
view_matrix = eval(view_matrix_string)
except Exception:
view_matrix = None
if view_matrix is not None:
self._glview.set_view_matrix(view_matrix)
else:
self.set_default_view()
def shutdown_plugin(self):
pass
# ==============================================
# QGLWidget requires
def set_default_view(self):
self._glview.makeCurrent()
self._glview.reset_view()
self._glview.rotate((0, 0, 1), 45)
self._glview.rotate((1, 0, 0), -45)
self._glview.translate((0, 0, -200))
def update_timeout(self):
self._glview.makeCurrent()
self._glview.updateGL()
glRotated(45, 0, 0, 1)
def glview_paintGL(self):
self._glview.paintGL_original()
# draw the axis, the plain and something
gl_painter.draw_basic_objects()
self.texture = self._glview.get_texture(self.qimage)
gl_painter.map_texture_on_sphere(self.texture, self.sphere_radius, 30,
30)
def glview_mouseReleaseEvent(self, event):
if event.button() == Qt.RightButton:
menu = QMenu(self._glview)
action = QAction(self._glview.tr("Reset view"), self._glview)
menu.addAction(action)
action.triggered.connect(self.set_default_view)
menu.exec_(self._glview.mapToGlobal(event.pos()))
def glview_wheelEvent(self, event):
# only zoom when no mouse buttons are pressed, to prevent interference with other user interactions
if event.buttons() == Qt.NoButton:
d = float(event.delta()) / 200.0 * self._glview._radius
# TODO: make the moving stop when going to the outside of the sphere
self._glview.translate([0.0, 0.0, d])
self._glview.updateGL()
event.accept()
# ==============================================
# initialize
def initialize_vals(self):
self.position = (0.0, 0.0, 0.0)
self.orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
self.topicName = None
self.subscriber = None
self.sphere_radius = 200
self.qimage = None
self.texture = None
self.filename = None
self.jpeg_data = None
def initialize_glview(self):
# create GL view
self._glview = MyGLWidget()
self._glview.setAcceptDrops(True)
# backup and replace original paint method
# self.glView.paintGL is callbacked from QGLWidget
self._glview.paintGL_original = self._glview.paintGL
self._glview.paintGL = self.glview_paintGL
# backup and replace original mouse release method
self._glview.mouseReleaseEvent_original = self._glview.mouseReleaseEvent
self._glview.mouseReleaseEvent = self.glview_mouseReleaseEvent
# backup and replace original mouse release method
self._glview.wheelEvent_original = self._glview.wheelEvent
self._glview.wheelEvent = self.glview_wheelEvent
# add GL view to widget layout
# http://doc.qt.io/qt-4.8/qgridlayout.html
self.layout().addWidget(self._glview, 1, 0, 1, 4)
self.qimage = QImage(self.filename, 'JPEG') # GL_TEXTURE_2D
# self.qimage = QImage('/home/matsunolab/Pictures/testimage_big.jpg', 'JPEG') # GL_TEXTURE_2D
def initialize_timer(self):
# updateTimeout is called with interval time
self.update_timer = QTimer(self)
self.update_timer.timeout.connect(self.update_timeout)
# init and start update timer with 40ms (25fps)
self.update_timer.start(40)
class SimpleRobotSteeringPlugin(Plugin):
DEFAULT_LINEAR_VELOCITY = 1.0
DEFAULT_ANGULAR_VELOCITY = math.pi / 2
def __init__(self, context):
super(SimpleRobotSteeringPlugin, self).__init__(context)
self.setObjectName('SimpleRobotSteeringPlugin')
self._publisher = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('bwi_rqt_plugins'), 'resource', 'SimpleRobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('SimpleRobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._widget.keyPressEvent = self.keyPressEvent
self._widget.keyReleaseEvent = self.keyReleaseEvent
context.add_widget(self._widget)
self._widget.topic_line_edit.textChanged.connect(self._on_topic_changed)
self.linear_vel = 0
self.angular_vel = 0
# After doing so, key press events seem to work ok.
self._widget.w_button.setFocus()
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(self._on_parameter_changed)
self._update_parameter_timer.start(100)
@Slot(str)
def _on_topic_changed(self, topic):
topic = str(topic)
self._unregister_publisher()
try:
self._publisher = rospy.Publisher(topic, Twist, queue_size=10)
except TypeError:
self._publisher = rospy.Publisher(topic, Twist)
def keyPressEvent(self, event):
if not event.isAutoRepeat():
if event.key() == Qt.Key_W:
self.linear_vel = SimpleRobotSteeringPlugin.DEFAULT_LINEAR_VELOCITY
self._widget.w_button.setDown(True)
self._widget.s_button.setDown(False)
elif event.key() == Qt.Key_S:
self.linear_vel = -SimpleRobotSteeringPlugin.DEFAULT_LINEAR_VELOCITY
self._widget.s_button.setDown(True)
self._widget.w_button.setDown(False)
elif event.key() == Qt.Key_A:
self.angular_vel = SimpleRobotSteeringPlugin.DEFAULT_ANGULAR_VELOCITY
self._widget.a_button.setDown(True)
self._widget.d_button.setDown(False)
elif event.key() == Qt.Key_D:
self.angular_vel = -SimpleRobotSteeringPlugin.DEFAULT_ANGULAR_VELOCITY
self._widget.d_button.setDown(True)
self._widget.a_button.setDown(False)
def keyReleaseEvent(self, event):
if not event.isAutoRepeat():
if self.linear_vel > 0 and event.key() == Qt.Key_W:
self.linear_vel = 0
self._widget.w_button.setDown(False)
elif self.linear_vel < 0 and event.key() == Qt.Key_S:
self.linear_vel = 0
self._widget.s_button.setDown(False)
elif self.angular_vel > 0 and event.key() == Qt.Key_A:
self.angular_vel = 0
self._widget.a_button.setDown(False)
elif self.angular_vel < 0 and event.key() == Qt.Key_D:
self.angular_vel = 0
self._widget.d_button.setDown(False)
def _on_parameter_changed(self):
self._widget.linear_speed.setText("%.2f"%self.linear_vel)
self._widget.angular_speed.setText("%.2f"%self.angular_vel)
self._send_twist(self.linear_vel, self.angular_vel)
def _send_twist(self, x_linear, z_angular):
if self._publisher is None:
return
twist = Twist()
twist.linear.x = x_linear
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = z_angular
self._publisher.publish(twist)
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
self._update_parameter_timer.stop()
self._unregister_publisher()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('topic' , self._widget.topic_line_edit.text())
def restore_settings(self, plugin_settings, instance_settings):
value = instance_settings.value('topic', "/cmd_vel")
value = rospy.get_param("~default_topic", value)
self._widget.topic_line_edit.setText(value)
class JointTrajectoryController(Plugin):
"""
Graphical frontend for a C{JointTrajectoryController}.
There are two modes for interacting with a controller:
1. B{Monitor mode} Joint displays are updated with the state reported
by the controller. This is a read-only mode and does I{not} send
control commands. Every time a new controller is selected, it starts
in monitor mode for safety reasons.
2. B{Control mode} Joint displays update the control command that is
sent to the controller. Commands are sent periodically evan if the
displays are not being updated by the user.
To control the aggressiveness of the motions, the maximum speed of the
sent commands can be scaled down using the C{Speed scaling control}
This plugin is able to detect and keep track of all active controller
managers, as well as the JointTrajectoryControllers that are I{running}
in each controller manager.
For a controller to be compatible with this plugin, it must comply with
the following requisites:
- The controller type contains the C{JointTrajectoryController}
substring, e.g., C{position_controllers/JointTrajectoryController}
- The controller exposes the C{command} and C{state} topics in its
ROS interface.
Additionally, there must be a URDF loaded with a valid joint limit
specification, namely position (when applicable) and velocity limits.
A reference implementation of the C{JointTrajectoryController} is
available in the C{joint_trajectory_controller} ROS package.
"""
_cmd_pub_freq = 10.0 # Hz
_widget_update_freq = 30.0 # Hz
_ctrlrs_update_freq = 1 # Hz
_min_traj_dur = 5.0 / _cmd_pub_freq # Minimum trajectory duration
jointStateChanged = Signal([dict])
def __init__(self, context):
super(JointTrajectoryController, self).__init__(context)
self.setObjectName('JointTrajectoryController')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'),
'resource', 'joint_trajectory_controller.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JointTrajectoryControllerUi')
ns = rospy.get_namespace()[1:-1]
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
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_jtc_list_timer = QTimer(self)
self._update_jtc_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()
# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber
self._list_controllers = None
def shutdown_plugin(self):
self._update_cmd_timer.stop()
self._update_act_pos_timer.stop()
self._update_cm_list_timer.stop()
self._update_jtc_list_timer.stop()
self._unregister_state_sub()
self._unregister_cmd_pub()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('cm_ns', self._cm_ns)
instance_settings.set_value('jtc_name', self._jtc_name)
def restore_settings(self, plugin_settings, instance_settings):
# Restore last session's controller_manager, if present
self._update_cm_list()
cm_ns = instance_settings.value('cm_ns')
cm_combo = self._widget.cm_combo
cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())]
try:
idx = cm_list.index(cm_ns)
cm_combo.setCurrentIndex(idx)
# Resore last session's controller, if running
self._update_jtc_list()
jtc_name = instance_settings.value('jtc_name')
jtc_combo = self._widget.jtc_combo
jtc_list = [
jtc_combo.itemText(i) for i in range(jtc_combo.count())
]
try:
idx = jtc_list.index(jtc_name)
jtc_combo.setCurrentIndex(idx)
except (ValueError):
pass
except (ValueError):
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget
# title bar
# Usually used to open a modal configuration dialog
def _update_cm_list(self):
update_combo(self._widget.cm_combo, self._list_cm())
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if not self._list_controllers:
self._widget.jtc_combo.clear()
return
# List of running controllers with a valid joint limits specification
# for _all_ their joints
running_jtc = self._running_jtc_info()
if running_jtc and not self._robot_joint_limits:
self._robot_joint_limits = get_joint_limits() # Lazy evaluation
valid_jtc = []
for jtc_info in running_jtc:
has_limits = all(name in self._robot_joint_limits
for name in _jtc_joint_names(jtc_info))
if has_limits:
valid_jtc.append(jtc_info)
valid_jtc_names = [data.name for data in valid_jtc]
# Update widget
update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))
def _on_speed_scaling_change(self, val):
self._speed_scale = val / self._speed_scaling_widget.slider.maximum()
def _on_joint_state_change(self, actual_pos):
assert (len(actual_pos) == len(self._joint_pos))
for name in actual_pos.keys():
self._joint_pos[name]['position'] = actual_pos[name]
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
if cm_ns:
self._list_controllers = ControllerLister(cm_ns)
# NOTE: Clear below is important, as different controller managers
# might have controllers with the same name but different
# configurations. Clearing forces controller re-discovery
self._widget.jtc_combo.clear()
self._update_jtc_list()
else:
self._list_controllers = None
def _on_jtc_change(self, jtc_name):
self._unload_jtc()
self._jtc_name = jtc_name
if self._jtc_name:
self._load_jtc()
def _on_jtc_enabled(self, val):
# Don't allow enabling if there are no controllers selected
if not self._jtc_name:
self._widget.enable_button.setChecked(False)
return
# Enable/disable joint displays
for joint_widget in self._joint_widgets():
joint_widget.setEnabled(val)
# Enable/disable speed scaling
self._speed_scaling_widget.setEnabled(val)
if val:
# Widgets send desired position commands to controller
self._update_act_pos_timer.stop()
self._update_cmd_timer.start()
else:
# Controller updates widgets with actual position
self._update_cmd_timer.stop()
self._update_act_pos_timer.start()
def _load_jtc(self):
# Initialize joint data corresponding to selected controller
running_jtc = self._running_jtc_info()
self._joint_names = next(
_jtc_joint_names(x) for x in running_jtc
if x.name == self._jtc_name)
for name in self._joint_names:
self._joint_pos[name] = {}
# Update joint display
try:
layout = self._widget.joint_group.layout()
for name in self._joint_names:
limits = self._robot_joint_limits[name]
joint_widget = DoubleEditor(limits['min_position'],
limits['max_position'])
layout.addRow(name, joint_widget)
# NOTE: Using partial instead of a lambda because lambdas
# "will not evaluate/look up the argument values before it is
# effectively called, breaking situations like using a loop
# variable inside it"
from functools import partial
par = partial(self._update_single_cmd_cb, name=name)
joint_widget.valueChanged.connect(par)
except:
# TODO: Can we do better than swallow the exception?
from sys import exc_info
print('Unexpected error:', exc_info()[0])
# Enter monitor mode (sending commands disabled)
self._on_jtc_enabled(False)
# Setup ROS interfaces
jtc_ns = _resolve_controller_ns(self._cm_ns, self._jtc_name)
state_topic = jtc_ns + '/state'
cmd_topic = jtc_ns + '/command'
self._state_sub = rospy.Subscriber(state_topic,
JointTrajectoryControllerState,
self._state_cb,
queue_size=1)
self._cmd_pub = rospy.Publisher(cmd_topic,
JointTrajectory,
queue_size=1)
# Start updating the joint positions
self.jointStateChanged.connect(self._on_joint_state_change)
def _unload_jtc(self):
# Stop updating the joint positions
try:
self.jointStateChanged.disconnect(self._on_joint_state_change)
except:
pass
# Reset ROS interfaces
self._unregister_state_sub()
self._unregister_cmd_pub()
# Clear joint widgets
# NOTE: Implementation is a workaround for:
# https://bugreports.qt-project.org/browse/QTBUG-15990 :(
layout = self._widget.joint_group.layout()
if layout is not None:
while layout.count():
layout.takeAt(0).widget().deleteLater()
# Delete existing layout by reparenting to temporary
QWidget().setLayout(layout)
self._widget.joint_group.setLayout(QFormLayout())
# Reset joint data
self._joint_names = []
self._joint_pos = {}
# Enforce monitor mode (sending commands disabled)
self._widget.enable_button.setChecked(False)
def _running_jtc_info(self):
from controller_manager_msgs.utils\
import filter_by_type, filter_by_state
controller_list = self._list_controllers()
jtc_list = filter_by_type(controller_list,
'JointTrajectoryController',
match_substring=True)
running_jtc_list = filter_by_state(jtc_list, 'running')
return running_jtc_list
def _unregister_cmd_pub(self):
if self._cmd_pub is not None:
self._cmd_pub.unregister()
self._cmd_pub = None
def _unregister_state_sub(self):
if self._state_sub is not None:
self._state_sub.unregister()
self._state_sub = None
def _state_cb(self, msg):
actual_pos = {}
for i in range(len(msg.joint_names)):
joint_name = msg.joint_names[i]
joint_pos = msg.actual.positions[i]
actual_pos[joint_name] = joint_pos
self.jointStateChanged.emit(actual_pos)
def _update_single_cmd_cb(self, val, name):
self._joint_pos[name]['command'] = val
def _update_cmd_cb(self):
dur = []
traj = JointTrajectory()
traj.joint_names = self._joint_names
point = JointTrajectoryPoint()
for name in traj.joint_names:
pos = self._joint_pos[name]['position']
cmd = pos
try:
cmd = self._joint_pos[name]['command']
except (KeyError):
pass
max_vel = self._robot_joint_limits[name]['max_velocity']
dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur))
point.positions.append(cmd)
point.time_from_start = rospy.Duration(max(dur) / self._speed_scale)
traj.points.append(point)
self._cmd_pub.publish(traj)
def _update_joint_widgets(self):
joint_widgets = self._joint_widgets()
for id in range(len(joint_widgets)):
joint_name = self._joint_names[id]
try:
joint_pos = self._joint_pos[joint_name]['position']
joint_widgets[id].setValue(joint_pos)
except (KeyError):
pass # Can happen when first connected to controller
def _joint_widgets(self): # TODO: Cache instead of compute every time?
widgets = []
layout = self._widget.joint_group.layout()
for row_id in range(layout.rowCount()):
widgets.append(
layout.itemAt(row_id, QFormLayout.FieldRole).widget())
return widgets
class Plot3DWidget(QWidget):
_redraw_interval = 40
def __init__(self, initial_topics=None, start_paused=False,
buffer_length=100, use_poly=True, no_legend=False):
super(Plot3DWidget, self).__init__()
self.setObjectName('Plot3DWidget')
self._buffer_length = buffer_length
self._initial_topics = initial_topics
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'),
'resource', 'plot3d.ui')
loadUi(ui_file, self)
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.remove_topic_button.setIcon(QIcon.fromTheme('remove'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatDataPlot3D(self, self._buffer_length,
use_poly, no_legend)
self.data_plot_layout.addWidget(self.data_plot)
self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked())
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self.subscribe_topic_button.setEnabled(False)
if start_paused:
self.pause_button.setChecked(True)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self._start_time = rospy.get_time()
self._rosdata = {}
self._remove_topic_menu = QMenu()
# init and start update timer for plot
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
if self._initial_topics:
for topic_name in self._initial_topics:
self.add_topic(topic_name)
self._initial_topics = None
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
# get topic name
if not event.mimeData().hasText():
if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
return
item = event.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name == None:
qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
return
else:
topic_name = str(event.mimeData().text())
# check for numeric field type
is_numeric, is_array, message = is_slot_numeric(topic_name)
if is_numeric and not is_array:
event.acceptProposedAction()
else:
qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message))
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.add_topic(topic_name)
@Slot(str)
def on_topic_edit_textChanged(self, topic_name):
# on empty topic name, update topics
if topic_name in ('', '/'):
self._topic_completer.update_topics()
is_numeric, is_array, message = is_slot_numeric(topic_name)
self.subscribe_topic_button.setEnabled(is_numeric and not is_array)
self.subscribe_topic_button.setToolTip(message)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.add_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.add_topic(str(self.topic_edit.text()))
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
@Slot(bool)
def on_autoscroll_checkbox_clicked(self, checked):
self.data_plot.autoscroll(checked)
@Slot()
def on_clear_button_clicked(self):
self.clean_up_subscribers()
def update_plot(self):
if self.data_plot is not None:
needs_redraw = False
for topic_name, rosdata in self._rosdata.items():
try:
data_x, data_y = rosdata.next()
if data_x or data_y:
self.data_plot.update_values(topic_name, data_x, data_y)
needs_redraw = True
except RosPlotException as e:
qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e)
if needs_redraw:
self.data_plot.redraw()
def _subscribed_topics_changed(self):
self._update_remove_topic_menu()
if not self.pause_button.isChecked():
# if pause button is not pressed, enable timer based on subscribed topics
self.enable_timer(self._rosdata)
def _update_remove_topic_menu(self):
def make_remove_topic_function(x):
return lambda: self.remove_topic(x)
self._remove_topic_menu.clear()
for topic_name in sorted(self._rosdata.keys()):
action = QAction(topic_name, self._remove_topic_menu)
action.triggered.connect(make_remove_topic_function(topic_name))
self._remove_topic_menu.addAction(action)
self.remove_topic_button.setMenu(self._remove_topic_menu)
def add_topic(self, topic_name):
if topic_name in self._rosdata:
qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name)
return
self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
if self._rosdata[topic_name].error is not None:
qWarning(str(self._rosdata[topic_name].error))
del self._rosdata[topic_name]
else:
data_x, data_y = self._rosdata[topic_name].next()
self.data_plot.add_curve(topic_name, topic_name, data_x, data_y)
self._subscribed_topics_changed()
def remove_topic(self, topic_name):
self._rosdata[topic_name].close()
del self._rosdata[topic_name]
self.data_plot.remove_curve(topic_name)
self._subscribed_topics_changed()
def clean_up_subscribers(self):
for topic_name, rosdata in self._rosdata.items():
rosdata.close()
self.data_plot.remove_curve(topic_name)
self._rosdata = {}
self._subscribed_topics_changed()
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
class RobotSteering(Plugin):
slider_factor = 1000.0
def __init__(self, context):
super(RobotSteering, self).__init__(context)
self.setObjectName('RobotSteering')
self._publisher = None
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_robot_steering'), 'resource',
'RobotSteering.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('RobotSteeringUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.type_combo_box.currentIndexChanged.connect(
self._on_type_changed)
self._widget.topic_line_edit.textChanged.connect(
self._on_topic_changed)
self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
self._widget.x_linear_slider.valueChanged.connect(
self._on_x_linear_slider_changed)
self._widget.z_angular_slider.valueChanged.connect(
self._on_z_angular_slider_changed)
self._widget.increase_x_linear_push_button.pressed.connect(
self._on_strong_increase_x_linear_pressed)
self._widget.reset_x_linear_push_button.pressed.connect(
self._on_reset_x_linear_pressed)
self._widget.decrease_x_linear_push_button.pressed.connect(
self._on_strong_decrease_x_linear_pressed)
self._widget.increase_z_angular_push_button.pressed.connect(
self._on_strong_increase_z_angular_pressed)
self._widget.reset_z_angular_push_button.pressed.connect(
self._on_reset_z_angular_pressed)
self._widget.decrease_z_angular_push_button.pressed.connect(
self._on_strong_decrease_z_angular_pressed)
self._widget.max_x_linear_double_spin_box.valueChanged.connect(
self._on_max_x_linear_changed)
self._widget.min_x_linear_double_spin_box.valueChanged.connect(
self._on_min_x_linear_changed)
self._widget.max_z_angular_double_spin_box.valueChanged.connect(
self._on_max_z_angular_changed)
self._widget.min_z_angular_double_spin_box.valueChanged.connect(
self._on_min_z_angular_changed)
self.shortcut_w = QShortcut(QKeySequence(Qt.Key_W), self._widget)
self.shortcut_w.setContext(Qt.ApplicationShortcut)
self.shortcut_w.activated.connect(self._on_increase_x_linear_pressed)
self.shortcut_x = QShortcut(QKeySequence(Qt.Key_X), self._widget)
self.shortcut_x.setContext(Qt.ApplicationShortcut)
self.shortcut_x.activated.connect(self._on_reset_x_linear_pressed)
self.shortcut_s = QShortcut(QKeySequence(Qt.Key_S), self._widget)
self.shortcut_s.setContext(Qt.ApplicationShortcut)
self.shortcut_s.activated.connect(self._on_decrease_x_linear_pressed)
self.shortcut_a = QShortcut(QKeySequence(Qt.Key_A), self._widget)
self.shortcut_a.setContext(Qt.ApplicationShortcut)
self.shortcut_a.activated.connect(self._on_increase_z_angular_pressed)
self.shortcut_z = QShortcut(QKeySequence(Qt.Key_Z), self._widget)
self.shortcut_z.setContext(Qt.ApplicationShortcut)
self.shortcut_z.activated.connect(self._on_reset_z_angular_pressed)
self.shortcut_d = QShortcut(QKeySequence(Qt.Key_D), self._widget)
self.shortcut_d.setContext(Qt.ApplicationShortcut)
self.shortcut_d.activated.connect(self._on_decrease_z_angular_pressed)
self.shortcut_shift_w = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_W),
self._widget)
self.shortcut_shift_w.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_w.activated.connect(
self._on_strong_increase_x_linear_pressed)
self.shortcut_shift_x = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_X),
self._widget)
self.shortcut_shift_x.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_x.activated.connect(
self._on_reset_x_linear_pressed)
self.shortcut_shift_s = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_S),
self._widget)
self.shortcut_shift_s.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_s.activated.connect(
self._on_strong_decrease_x_linear_pressed)
self.shortcut_shift_a = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_A),
self._widget)
self.shortcut_shift_a.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_a.activated.connect(
self._on_strong_increase_z_angular_pressed)
self.shortcut_shift_z = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Z),
self._widget)
self.shortcut_shift_z.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_z.activated.connect(
self._on_reset_z_angular_pressed)
self.shortcut_shift_d = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_D),
self._widget)
self.shortcut_shift_d.setContext(Qt.ApplicationShortcut)
self.shortcut_shift_d.activated.connect(
self._on_strong_decrease_z_angular_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.Key_Space),
self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self.shortcut_space = QShortcut(QKeySequence(Qt.SHIFT + Qt.Key_Space),
self._widget)
self.shortcut_space.setContext(Qt.ApplicationShortcut)
self.shortcut_space.activated.connect(self._on_stop_pressed)
self._widget.stop_push_button.setToolTip(
self._widget.stop_push_button.toolTip() + ' ' +
self.tr('([Shift +] Space)'))
self._widget.increase_x_linear_push_button.setToolTip(
self._widget.increase_x_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] W)'))
self._widget.reset_x_linear_push_button.setToolTip(
self._widget.reset_x_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] X)'))
self._widget.decrease_x_linear_push_button.setToolTip(
self._widget.decrease_x_linear_push_button.toolTip() + ' ' +
self.tr('([Shift +] S)'))
self._widget.increase_z_angular_push_button.setToolTip(
self._widget.increase_z_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] A)'))
self._widget.reset_z_angular_push_button.setToolTip(
self._widget.reset_z_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] Z)'))
self._widget.decrease_z_angular_push_button.setToolTip(
self._widget.decrease_z_angular_push_button.toolTip() + ' ' +
self.tr('([Shift +] D)'))
# timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self)
self._update_parameter_timer.timeout.connect(
self._on_parameter_changed)
self._update_parameter_timer.start(100)
self.zero_cmd_sent = False
@Slot(str)
def _on_topic_changed(self, topic):
topic = str(topic)
self._unregister_publisher()
if topic == '':
return
msg_type = Twist if self._widget.type_combo_box.currentIndex(
) == 0 else AckermannDrive
try:
self._publisher = rospy.Publisher(topic, msg_type, queue_size=10)
except TypeError:
self._publisher = rospy.Publisher(topic, msg_type)
def _on_type_changed(self, index):
"""
If an element in the type combo box is changed, this method is called with the selected index as a parameter.
:param index: The current selected index. '0' means Twist and '1' means Ackermann type of message.
"""
# If the type changed, automatically set the topic-name to default value
if self._widget.type_combo_box.currentIndex() == 0:
self._widget.topic_line_edit.setText("/cmd_vel")
else:
self._widget.topic_line_edit.setText("/cmd_ackermann")
self._on_topic_changed(self._widget.topic_line_edit.text())
def _on_stop_pressed(self):
# If the current value of sliders is zero directly send stop twist msg
if self._widget.x_linear_slider.value() is 0 and \
self._widget.z_angular_slider.value() is 0:
self.zero_cmd_sent = False
self._on_parameter_changed()
else:
self._widget.x_linear_slider.setValue(0)
self._widget.z_angular_slider.setValue(0)
def _on_x_linear_slider_changed(self):
self._widget.current_x_linear_label.setText(
'%0.2f m/s' % (self._widget.x_linear_slider.value() /
RobotSteering.slider_factor))
self._on_parameter_changed()
def _on_z_angular_slider_changed(self):
format_str = '%0.2f rad/s' if self._widget.type_combo_box.currentIndex(
) == 0 else '%0.2f rad'
self._widget.current_z_angular_label.setText(
format_str % (self._widget.z_angular_slider.value() /
RobotSteering.slider_factor))
self._on_parameter_changed()
def _on_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() +
self._widget.x_linear_slider.singleStep())
def _on_reset_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(0)
def _on_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() -
self._widget.x_linear_slider.singleStep())
def _on_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() +
self._widget.z_angular_slider.singleStep())
def _on_reset_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(0)
def _on_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() -
self._widget.z_angular_slider.singleStep())
def _on_max_x_linear_changed(self, value):
self._widget.x_linear_slider.setMaximum(value *
RobotSteering.slider_factor)
def _on_min_x_linear_changed(self, value):
self._widget.x_linear_slider.setMinimum(value *
RobotSteering.slider_factor)
def _on_max_z_angular_changed(self, value):
self._widget.z_angular_slider.setMaximum(value *
RobotSteering.slider_factor)
def _on_min_z_angular_changed(self, value):
self._widget.z_angular_slider.setMinimum(value *
RobotSteering.slider_factor)
def _on_strong_increase_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() +
self._widget.x_linear_slider.pageStep())
def _on_strong_decrease_x_linear_pressed(self):
self._widget.x_linear_slider.setValue(
self._widget.x_linear_slider.value() -
self._widget.x_linear_slider.pageStep())
def _on_strong_increase_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() +
self._widget.z_angular_slider.pageStep())
def _on_strong_decrease_z_angular_pressed(self):
self._widget.z_angular_slider.setValue(
self._widget.z_angular_slider.value() -
self._widget.z_angular_slider.pageStep())
def _on_parameter_changed(self):
cmd_type = self._widget.type_combo_box.currentIndex()
if cmd_type == 0:
self._send_twist(
self._widget.x_linear_slider.value() /
RobotSteering.slider_factor,
self._widget.z_angular_slider.value() /
RobotSteering.slider_factor)
elif cmd_type == 1:
self._send_ackermann(
self._widget.x_linear_slider.value() /
RobotSteering.slider_factor,
self._widget.z_angular_slider.value() /
RobotSteering.slider_factor)
def _send_twist(self, x_linear, z_angular):
if self._publisher is None:
return
twist = Twist()
twist.linear.x = x_linear
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = z_angular
# Only send the zero command once so other devices can take control
if x_linear == 0 and z_angular == 0:
if not self.zero_cmd_sent:
self.zero_cmd_sent = True
self._publisher.publish(twist)
else:
self.zero_cmd_sent = False
self._publisher.publish(twist)
def _send_ackermann(self, x_linear, z_angular):
if self._publisher is None:
return
ackermann_cmd = AckermannDrive()
ackermann_cmd.speed = x_linear
ackermann_cmd.steering_angle = z_angular
# Only send the zero command once so other devices can take control
if x_linear == 0 and z_angular == 0:
if not self.zero_cmd_sent:
self.zero_cmd_sent = True
self._publisher.publish(ackermann_cmd)
else:
self.zero_cmd_sent = False
self._publisher.publish(ackermann_cmd)
def _unregister_publisher(self):
if self._publisher is not None:
self._publisher.unregister()
self._publisher = None
def shutdown_plugin(self):
self._update_parameter_timer.stop()
self._unregister_publisher()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('topic',
self._widget.topic_line_edit.text())
instance_settings.set_value(
'vx_max', self._widget.max_x_linear_double_spin_box.value())
instance_settings.set_value(
'vx_min', self._widget.min_x_linear_double_spin_box.value())
instance_settings.set_value(
'vw_max', self._widget.max_z_angular_double_spin_box.value())
instance_settings.set_value(
'vw_min', self._widget.min_z_angular_double_spin_box.value())
instance_settings.set_value('cmd_type',
self._widget.type_combo_box.currentIndex())
def restore_settings(self, plugin_settings, instance_settings):
value = instance_settings.value('topic', "/cmd_vel")
value = rospy.get_param("~default_topic", value)
self._widget.topic_line_edit.setText(value)
value = self._widget.max_x_linear_double_spin_box.value()
value = instance_settings.value('vx_max', value)
value = rospy.get_param("~default_vx_max", value)
self._widget.max_x_linear_double_spin_box.setValue(float(value))
value = self._widget.min_x_linear_double_spin_box.value()
value = instance_settings.value('vx_min', value)
value = rospy.get_param("~default_vx_min", value)
self._widget.min_x_linear_double_spin_box.setValue(float(value))
value = self._widget.max_z_angular_double_spin_box.value()
value = instance_settings.value('vw_max', value)
value = rospy.get_param("~default_vw_max", value)
self._widget.max_z_angular_double_spin_box.setValue(float(value))
value = self._widget.min_z_angular_double_spin_box.value()
value = instance_settings.value('vw_min', value)
value = rospy.get_param("~default_vw_min", value)
self._widget.min_z_angular_double_spin_box.setValue(float(value))
value = self._widget.type_combo_box.currentIndex()
value = instance_settings.value('cmd_type', value)
self._widget.type_combo_box.setCurrentIndex(int(value))
class SysmonWidget(QWidget):
_last_info_msg = SystemInfo()
_last_tracker_msg = TrackerState()
_slam_info_sub = None
_slam_tracker_sub = None
_slam_init_srv = None
_slam_reset_srv = None
_mcptam_namespace = None
_num_received_msgs = 0
def __init__(self, mcptam_namespace='mcptam'):
# Init QWidget
super(SysmonWidget, self).__init__()
self.setObjectName('SysmonWidget')
# load UI
ui_file = os.path.join(rospkg.RosPack().get_path('artemis_sysmon'), 'resource', 'widget.ui')
loadUi(ui_file, self)
# Subscribe to ROS topics and register callbacks
self._slam_info_sub = rospy.Subscriber(mcptam_namespace+'/system_info', SystemInfo, self.slam_info_cb)
self._slam_tracker_sub = rospy.Subscriber(mcptam_namespace+'/tracker_state', TrackerState, self.slam_tracker_cb)
# Initialize service call
print('Waiting for MAV services')
rospy.wait_for_service(mcptam_namespace+'/init')
rospy.wait_for_service(mcptam_namespace+'/reset')
print('Connected to SLAM system')
self._slam_init_srv = rospy.ServiceProxy(mcptam_namespace+'/init', Empty)
self._slam_reset_srv = rospy.ServiceProxy(mcptam_namespace+'/reset', Reset)
# init and start update timer for data, the timer calls the function update_info all 40ms
self._update_info_timer = QTimer(self)
self._update_info_timer.timeout.connect(self.update_info)
self._update_info_timer.start(40)
# set the functions that are called when a button is pressed
self.button_start.pressed.connect(self.on_start_button_pressed)
self.button_reset.pressed.connect(self.on_reset_button_pressed)
self.button_quit.pressed.connect(self.on_quit_button_pressed)
@Slot(str)
def slam_info_cb(self, msg):
self._last_info_msg = msg
self._num_received_msgs += 1
def slam_tracker_cb(self, msg):
self._last_tracker_msg = msg
self._num_received_msgs += 1
def update_info(self):
info_text = 'Waiting for MAV connection'
if self._num_received_msgs > 0:
# Tracker FPS
info_text = 'fps = %.2f' % self._last_info_msg.dFPS
info_text += '\n'
# System State
info_text += self._last_info_msg.message
info_text += '\n'
# set info text
self.mcptam_info_label.setText(info_text)
def on_start_button_pressed(self):
print('Requesting SLAM init')
self.slam_init()
def on_reset_button_pressed(self):
print('Requesting SLAM reset')
self.slam_reset()
def on_quit_button_pressed(self):
print('Quitting SLAM')
self.slam_quit()
def slam_init(self):
try:
r = _slam_init_srv() # handle response TODO
except rospy.ServiceException as exc:
print("Init request failure" + str(exc))
def slam_reset(self):
try:
r = _slam_reset_srv(true, false) # handle response TODO
except rospy.ServiceException as exc:
print("Reset request failure" + str(exc))
def slam_quit(self):
print("Unimplemented")
class Theta360ViewWidget(QWidget):
def __init__(self, plugin):
super(Theta360ViewWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('theta_viewer'), 'resource',
'Theta360ViewWidget.ui')
loadUi(ui_file, self)
self.plugin = plugin
self.position = (0.0, 0.0, 0.0)
self.orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
self.topicName = None
self.subscriber = None
# create GL view
self._glview = MyGLWidget()
self._glview.setAcceptDrops(True)
# backup and replace original paint method
# self.glView.paintGL is callbacked from QGLWidget
self._glview.paintGL_original = self._glview.paintGL
self._glview.paintGL = self.glview_paintGL
# backup and replace original mouse release method
self._glview.mouseReleaseEvent_original = self._glview.mouseReleaseEvent
self._glview.mouseReleaseEvent = self.glview_mouseReleaseEvent
# add GL view to widget layout
self.layout().addWidget(self._glview)
# init and start update timer with 40ms (25fps)
# updateTimeout is called with interval time
self.update_timer = QTimer(self)
self.update_timer.timeout.connect(self.update_timeout)
self.update_timer.start(40)
self.cnt = 0
## ==============================================
## rqt requires
def save_settings(self, plugin_settings, instance_settings):
view_matrix_string = repr(self._glview.get_view_matrix())
instance_settings.set_value('view_matrix', view_matrix_string)
def restore_settings(self, plugin_settings, instance_settings):
view_matrix_string = instance_settings.value('view_matrix')
try:
view_matrix = eval(view_matrix_string)
except Exception:
view_matrix = None
if view_matrix is not None:
self._glview.set_view_matrix(view_matrix)
else:
self.set_default_view()
def shutdown_plugin(self):
self.unregister_topic()
## ==============================================
## QGLWidget requires
def set_default_view(self):
self._glview.makeCurrent()
self._glview.reset_view()
self._glview.rotate((0, 0, 1), 45)
self._glview.rotate((1, 0, 0), -45)
self._glview.translate((0, 0, -200))
def update_timeout(self):
print self.cnt
self._glview.makeCurrent()
self._glview.updateGL()
glRotated(45 + self.cnt, 0, 0, 1)
def glview_paintGL(self):
self._glview.paintGL_original()
self.draw_basic_objects()
if self.cnt == 0:
print 'DRAW!'
self.qimage = QImage('/home/matsunolab/Pictures/testimage_big.jpg',
'JPEG') # GL_TEXTURE_2D
self.texture = self._glview.get_texture(self.qimage)
my_glfuncs.map_texture_on_sphere2(self.texture, 1500, 30, 30)
self.cnt += 1
def glview_mouseReleaseEvent(self, event):
if event.button() == Qt.RightButton:
menu = QMenu(self._glview)
action = QAction(self._glview.tr("Reset view"), self._glview)
menu.addAction(action)
action.triggered.connect(self.set_default_view)
menu.exec_(self._glview.mapToGlobal(event.pos()))
## ==============================================
## ROS requires
def message_callback(self, message):
self.position = (message.position.x, message.position.y,
message.position.z)
self.orientation = (message.orientation.x, message.orientation.y,
message.orientation.z, message.orientation.w)
def unregister_topic(self):
if self.subscriber:
self.subscriber.unregister()
def subscribe_topic(self, topicName):
msgClass, self.topicName, _ = get_topic_class(topicName)
self.subscriber = rospy.Subscriber(self.topicName, msgClass,
self.message_callback)
## ==============================================
## QT requires(?)
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
if not event.mimeData().hasText():
if not hasattr(event.source(), 'selectedItems') or len(
event.source().selectedItems()) == 0:
qWarning(
'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0'
)
return
item = event.source().selectedItems()[0]
ros_topic_name = item.data(0, Qt.UserRole)
if ros_topic_name is None:
qWarning(
'Plot.dragEnterEvent(): not hasattr(item, ros_topicName_)')
return
# TODO: do some checks for valid topic here
event.acceptProposedAction()
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.unregister_topic()
self.subscribe_topic(topic_name)
## ==============================================
## For drawing
def draw_basic_objects(self):
glLineWidth(5)
my_glfuncs.draw_axis()
glLineWidth(1)
glColor4f(1.0, 1.0, 1.0, 1.0)
my_glfuncs.draw_grand_gradation(200, 200, 10, 2)
class ImageView2Widget(QWidget):
"""
Qt widget to communicate with image_view2
"""
cv_image = None
pixmap = None
def __init__(self):
super(ImageView2Widget, self).__init__()
self.left_button_clicked = False
self.lock = Lock()
self.event_pub = rospy.Publisher("event", MouseEvent)
self.bridge = CvBridge()
self.label = ScaledLabel()
self.label.setAlignment(Qt.AlignCenter)
self.label.setSizePolicy(QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored))
#self.label.installEventFilter(self)
hbox = QtGui.QHBoxLayout(self)
hbox.addWidget(self.label)
self.setLayout(hbox)
self.image_sub = rospy.Subscriber("image_marked", Image,
self.imageCallback)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.redraw)
self._update_plot_timer.start(40)
self.setMouseTracking(True)
self.show()
def imageCallback(self, msg):
with self.lock:
cv_image = self.bridge.imgmsg_to_cv2(msg, msg.encoding)
if msg.encoding == "bgr8":
self.cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
elif msg.encoding == "rgb8":
self.cv_image = cv_image
self.numpy_image = np.asarray(self.cv_image)
def redraw(self):
with self.lock:
if self.cv_image != None:
size = self.cv_image.shape
img = QImage(self.cv_image.data,
size[1], size[0], size[2] * size[1],
QImage.Format_RGB888)
# convert to QPixmap
self.pixmap = QPixmap(size[1], size[0])
self.pixmap.convertFromImage(img)
self.label.setPixmap(self.pixmap.scaled(
self.label.width(), self.label.height(),
QtCore.Qt.KeepAspectRatio))
#self.label.setPixmap(self.pixmap)
def mousePosition(self, e):
label_x = self.label.x()
label_y = self.label.y()
label_width = self.label.width()
label_height = self.label.height()
pixmap_width = self.label.pixmap().width()
pixmap_height = self.label.pixmap().height()
x_offset = (label_width - pixmap_width) / 2.0 + label_x
y_offset = (label_height - pixmap_height) / 2.0 + label_y
return (e.x() - x_offset, e.y()- y_offset)
def mouseMoveEvent(self, e):
if self.left_button_clicked:
msg = MouseEvent()
msg.header.stamp = rospy.Time.now()
msg.type = MouseEvent.MOUSE_MOVE
msg.x, msg.y = self.mousePosition(e)
msg.width = self.label.pixmap().width()
msg.height = self.label.pixmap().height()
self.event_pub.publish(msg)
def mousePressEvent(self, e):
msg = MouseEvent()
msg.header.stamp = rospy.Time.now()
if e.button() == Qt.LeftButton:
msg.type = MouseEvent.MOUSE_LEFT_DOWN
self.left_button_clicked = True
elif msg.type == Qt.RightButton:
msg.type = MouseEvent.MOUSE_RIGHT_DOWN
msg.width = self.label.pixmap().width()
msg.height = self.label.pixmap().height()
msg.x, msg.y = self.mousePosition(e)
self.event_pub.publish(msg)
def mouseReleaseEvent(self, e):
if e.button() == Qt.LeftButton:
self.left_button_clicked = False
msg = MouseEvent()
msg.header.stamp = rospy.Time.now()
msg.width = self.label.pixmap().width()
msg.height = self.label.pixmap().height()
msg.type = MouseEvent.MOUSE_LEFT_UP
msg.x, msg.y = self.mousePosition(e)
self.event_pub.publish(msg)
def eventFilter(self, widget, event):
if not self.pixmap:
return QtGui.QMainWindow.eventFilter(self, widget, event)
if (event.type() == QtCore.QEvent.Resize and
widget is self.label):
self.label.setPixmap(self.pixmap.scaled(
self.label.width(), self.label.height(),
QtCore.Qt.KeepAspectRatio))
return True
return QtGui.QMainWindow.eventFilter(self, widget, event)
class PoseViewWidget(QWidget):
def __init__(self, plugin):
super(PoseViewWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_pose_view'), 'resource', 'PoseViewWidget.ui')
loadUi(ui_file, self)
self._plugin = plugin
self._position = (0.0, 0.0, 0.0)
self._orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
self._topic_name = None
self._subscriber = None
# create GL view
self._gl_view = GLWidget()
self._gl_view.setAcceptDrops(True)
# backup and replace original paint method
self._gl_view.paintGL_original = self._gl_view.paintGL
self._gl_view.paintGL = self._gl_view_paintGL
# backup and replace original mouse release method
self._gl_view.mouseReleaseEvent_original = self._gl_view.mouseReleaseEvent
self._gl_view.mouseReleaseEvent = self._gl_view_mouseReleaseEvent
# add GL view to widget layout
self.layout().addWidget(self._gl_view)
# init and start update timer with 40ms (25fps)
self._update_timer = QTimer(self)
self._update_timer.timeout.connect(self.update_timeout)
self._update_timer.start(40)
def save_settings(self, plugin_settings, instance_settings):
view_matrix_string = repr(self._gl_view.get_view_matrix())
instance_settings.set_value('view_matrix', view_matrix_string)
def restore_settings(self, plugin_settings, instance_settings):
view_matrix_string = instance_settings.value('view_matrix')
try:
view_matrix = eval(view_matrix_string)
except Exception:
view_matrix = None
if view_matrix is not None:
self._gl_view.set_view_matrix(view_matrix)
else:
self._set_default_view()
def _set_default_view(self):
self._gl_view.makeCurrent()
self._gl_view.reset_view()
self._gl_view.rotate((0, 0, 1), 45)
self._gl_view.rotate((1, 0, 0), -45)
self._gl_view.translate((0, -3, -15))
def message_callback_pose(self, message):
self._position = (message.position.x, message.position.y, message.position.z)
self._orientation = (message.orientation.x, message.orientation.y, message.orientation.z, message.orientation.w)
def message_callback_quaternion(self, message):
self._position = (0, 0, 0)
self._orientation = (message.x, message.y, message.z, message.w)
def update_timeout(self):
self._gl_view.makeCurrent()
self._gl_view.updateGL()
def _gl_view_paintGL(self):
self._gl_view.paintGL_original()
self._paintGLGrid()
self._paintGLCoorsystem()
self._paintGLBox()
def _paintGLBox(self):
self._position = (2.0, 2.0, 2.0) # Set fixed translation for now
glTranslatef(*self._position) # Translate Box
matrix = quaternion_matrix(self._orientation) # convert quaternion to translation matrix
glMultMatrixf(matrix) # Rotate Box
glBegin(GL_QUADS) # Start Drawing The Box
glColor3f(0.0, 1.0, 0.0)
glVertex3f(1.0, 1.0, -1.0) # Top Right Of The Quad (Top)
glVertex3f(-1.0, 1.0, -1.0) # Top Left Of The Quad (Top)
glVertex3f(-1.0, 1.0, 1.0) # Bottom Left Of The Quad (Top)
glVertex3f(1.0, 1.0, 1.0) # Bottom Right Of The Quad (Top)
glColor3f(0.5, 1.0, 0.5)
glVertex3f(1.0, -1.0, 1.0) # Top Right Of The Quad (Bottom)
glVertex3f(-1.0, -1.0, 1.0) # Top Left Of The Quad (Bottom)
glVertex3f(-1.0, -1.0, -1.0) # Bottom Left Of The Quad (Bottom)
glVertex3f(1.0, -1.0, -1.0) # Bottom Right Of The Quad (Bottom)
glColor3f(0.0, 0.0, 1.0)
glVertex3f(1.0, 1.0, 1.0) # Top Right Of The Quad (Front)
glVertex3f(-1.0, 1.0, 1.0) # Top Left Of The Quad (Front)
glVertex3f(-1.0, -1.0, 1.0) # Bottom Left Of The Quad (Front)
glVertex3f(1.0, -1.0, 1.0) # Bottom Right Of The Quad (Front)
glColor3f(0.5, 0.5, 1.0)
glVertex3f(1.0, -1.0, -1.0) # Bottom Left Of The Quad (Back)
glVertex3f(-1.0, -1.0, -1.0) # Bottom Right Of The Quad (Back)
glVertex3f(-1.0, 1.0, -1.0) # Top Right Of The Quad (Back)
glVertex3f(1.0, 1.0, -1.0) # Top Left Of The Quad (Back)
glColor3f(1.0, 0.5, 0.5)
glVertex3f(-1.0, 1.0, 1.0) # Top Right Of The Quad (Left)
glVertex3f(-1.0, 1.0, -1.0) # Top Left Of The Quad (Left)
glVertex3f(-1.0, -1.0, -1.0) # Bottom Left Of The Quad (Left)
glVertex3f(-1.0, -1.0, 1.0) # Bottom Right Of The Quad (Left)
glColor3f(1.0, 0.0, 0.0)
glVertex3f(1.0, 1.0, -1.0) # Top Right Of The Quad (Right)
glVertex3f(1.0, 1.0, 1.0) # Top Left Of The Quad (Right)
glVertex3f(1.0, -1.0, 1.0) # Bottom Left Of The Quad (Right)
glVertex3f(1.0, -1.0, -1.0) # Bottom Right Of The Quad (Right)
glEnd() # Done Drawing The Quad
def _paintGLGrid(self):
resolutionMillimeters = 1
griddedAreaSize = 100
glLineWidth(1.0)
glBegin(GL_LINES)
glColor3f(1.0, 1.0, 1.0)
glVertex3f(griddedAreaSize, 0, 0)
glVertex3f(-griddedAreaSize, 0, 0)
glVertex3f(0, griddedAreaSize, 0)
glVertex3f(0, -griddedAreaSize, 0)
numOfLines = int(griddedAreaSize / resolutionMillimeters)
for i in range(numOfLines):
glVertex3f(resolutionMillimeters * i, -griddedAreaSize, 0)
glVertex3f(resolutionMillimeters * i, griddedAreaSize, 0)
glVertex3f(griddedAreaSize, resolutionMillimeters * i, 0)
glVertex3f(-griddedAreaSize, resolutionMillimeters * i, 0)
glVertex3f(resolutionMillimeters * (-i), -griddedAreaSize, 0)
glVertex3f(resolutionMillimeters * (-i), griddedAreaSize, 0)
glVertex3f(griddedAreaSize, resolutionMillimeters * (-i), 0)
glVertex3f(-griddedAreaSize, resolutionMillimeters * (-i), 0)
glEnd()
def _paintGLCoorsystem(self):
glLineWidth(10.0)
glBegin(GL_LINES)
glColor3f(1.0, 0.0, 0.0)
glVertex3f(0.0, 0.0, 0.0)
glVertex3f(1.0, 0.0, 0.0)
glColor3f(0.0, 1.0, 0.0)
glVertex3f(0.0, 0.0, 0.0)
glVertex3f(0.0, 1.0, 0.0)
glColor3f(0.0, 0.0, 1.0)
glVertex3f(0.0, 0.0, 0.0)
glVertex3f(0.0, 0.0, 1.0)
glEnd()
def _gl_view_mouseReleaseEvent(self, event):
if event.button() == Qt.RightButton:
menu = QMenu(self._gl_view)
action = QAction(self._gl_view.tr("Reset view"), self._gl_view)
menu.addAction(action)
action.triggered.connect(self._set_default_view)
menu.exec_(self._gl_view.mapToGlobal(event.pos()))
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
if not event.mimeData().hasText():
if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
return
item = event.source().selectedItems()[0]
ros_topic_name = item.data(0, Qt.UserRole)
if ros_topic_name is None:
qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
return
# TODO: do some checks for valid topic here
event.acceptProposedAction()
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.unregister_topic()
self.subscribe_topic(topic_name)
def unregister_topic(self):
if self._subscriber:
self._subscriber.unregister()
def subscribe_topic(self, topic_name):
msg_class, self._topic_name, _ = get_topic_class(topic_name)
if (msg_class.__name__ == 'Quaternion') :
self._subscriber = rospy.Subscriber(self._topic_name, msg_class, self.message_callback_quaternion)
else :
self._subscriber = rospy.Subscriber(self._topic_name, msg_class, self.message_callback_pose)
def shutdown_plugin(self):
self.unregister_topic()
class StatusLightWidget(QWidget):
_UNKNOWN_COLOR = QColor("#dddddd")
_SUCCESS_COLOR = QColor("#18FFFF")
_WARN_COLOR = QColor("#FFCA00")
_ERROR_COLOR = QColor("#F44336")
def __init__(self):
super(StatusLightWidget, self).__init__()
self.lock = Lock()
self.status_sub = None
self.status = 0
self._status_topics = []
self._update_topic_timer = QTimer(self)
self._update_topic_timer.timeout.connect(self.updateTopics)
self._update_topic_timer.start(1000)
self._active_topic = None
self._dialog = ComboBoxDialog()
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.redraw)
self._update_plot_timer.start(1000 / 15)
def redraw(self):
self.update()
def paintEvent(self, event):
with self.lock:
if self.status == 1:
color = self._SUCCESS_COLOR
elif self.status == 2:
color = self._WARN_COLOR
else:
color = self._UNKNOWN_COLOR
rect = event.rect()
qp = QPainter()
qp.begin(self)
radius = min(rect.width(), rect.height()) - 100
qp.setFont(QFont('Helvetica', 100))
qp.setPen(QPen(QBrush(color), 50))
qp.setBrush(color)
qp.drawEllipse((rect.width() - radius) / 2, (rect.height() - radius) / 2,
radius, radius)
qp.end()
return
def trigger_configuration(self):
self._dialog.exec_()
self.setupSubscriber(self._status_topics[self._dialog.number])
def updateTopics(self):
need_to_update = False
for (topic, topic_type) in rospy.get_published_topics():
if topic_type == "std_msgs/UInt8":
if not topic in self._status_topics:
self._status_topics.append(topic)
need_to_update = True
if need_to_update:
self._status_topics = sorted(self._status_topics)
self._dialog.combo_box.clear()
for topic in self._status_topics:
self._dialog.combo_box.addItem(topic)
if self._active_topic:
if self._active_topic not in self._status_topics:
self._status_topics.append(self._active_topic)
self._dialog.combo_box.addItem(self._active_topic)
self._dialog.combo_box.setCurrentIndex(self._status_topics.index(self._active_topic))
def setupSubscriber(self, topic):
if self.status_sub:
self.status_sub.unregister()
self.status_sub = rospy.Subscriber(topic, UInt8,
self.statusCallback)
self._active_topic = topic
def onActivated(self, number):
self.setupSubscriber(self._status_topics[number])
def statusCallback(self, msg):
self.status = msg.data
def save_settings(self, plugin_settings, instance_settings):
if self._active_topic:
instance_settings.set_value("active_topic", self._active_topic)
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.value("active_topic"):
topic = instance_settings.value("active_topic")
self._dialog.combo_box.addItem(topic)
self.setupSubscriber(topic)
class Plot2DWidget(QWidget):
_redraw_interval = 40
def __init__(self, topics):
super(Plot2DWidget, self).__init__()
self.setObjectName("Plot2DWidget")
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path("jsk_rqt_plugins"), "resource", "plot_histogram.ui")
loadUi(ui_file, self)
self.cv_bridge = CvBridge()
self.subscribe_topic_button.setIcon(QIcon.fromTheme("add"))
self.pause_button.setIcon(QIcon.fromTheme("media-playback-pause"))
self.clear_button.setIcon(QIcon.fromTheme("edit-clear"))
self.data_plot = MatPlot2D(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
@Slot("QDropEvent*")
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.subscribe_topic(topic_name)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.subscribe_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.subscribe_topic(str(self.topic_edit.text()))
def subscribe_topic(self, topic_name):
self.topic_with_field_name = topic_name
self.pub_image = rospy.Publisher(topic_name + "/histogram_image", Image)
if not self._rosdata:
self._rosdata = ROSData(topic_name, self._start_time)
else:
if self._rosdata != topic_name:
self._rosdata.close()
self.data_plot.clear()
self._rosdata = ROSData(topic_name, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic_name)
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
@Slot()
def on_clear_button_clicked(self):
self.data_plot.clear()
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
def update_plot(self):
if not self._rosdata:
return
data_x, data_y = self._rosdata.next()
if len(data_y) == 0:
return
axes = self.data_plot._canvas.axes
axes.cla()
# matplotlib
# concatenate x and y in order to sort
concatenated_data = zip(data_y[-1].xs, data_y[-1].ys)
if self.sort_x:
concatenated_data.sort(key=lambda x: x[0])
xs = [d[0] for d in concatenated_data]
ys = [d[1] for d in concatenated_data]
if self.is_line:
axes.plot(xs, ys, label=self.topic_with_field_name)
else:
axes.scatter(xs, ys)
# set limit
axes.set_xlim(min(xs), max(xs))
axes.set_ylim(min(ys), max(ys))
# line fitting
if self.fit_line:
X = np.array(data_y[-1].xs)
Y = np.array(data_y[-1].ys)
A = np.array([X, np.ones(len(X))])
A = A.T
a, b = np.linalg.lstsq(A, Y)[0]
axes.plot(X, (a * X + b), "g--", label="{0} x + {1}".format(a, b))
if self.fit_line_ransac:
model_ransac = linear_model.RANSACRegressor(
linear_model.LinearRegression(), min_samples=2, residual_threshold=self.fit_line_ransac_outlier
)
X = np.array(data_y[-1].xs).reshape((len(data_y[-1].xs), 1))
Y = np.array(data_y[-1].ys)
model_ransac.fit(X, Y)
line_X = X
line_y_ransac = model_ransac.predict(line_X)
axes.plot(
line_X,
line_y_ransac,
"r--",
label="{0} x + {1}".format(model_ransac.estimator_.coef_[0][0], model_ransac.estimator_.intercept_[0]),
)
if not self.no_legend:
axes.legend(prop={"size": "8"})
axes.grid()
if self.xtitle:
axes.set_xlabel(self.xtitle)
if self.ytitle:
axes.set_ylabel(self.ytitle)
self.data_plot._canvas.draw()
buffer = StringIO()
self.data_plot._canvas.figure.savefig(buffer, format="png")
buffer.seek(0)
img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR)
self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class TelloPlugin(Plugin):
STATE_DISCONNECTED = state.State('disconnected')
STATE_CONNECTING = state.State('connecting')
STATE_CONNECTED = state.State('connected')
STATE_FLYING = state.State('flying')
STATE_QUIT = state.State('quit')
# define signals ROS updates
sig_connection_changed = pyqtSignal()
sig_update_position = pyqtSignal()
sig_battery_percentage = pyqtSignal(int)
sig_tello_link = pyqtSignal(int)
sig_wifi_sig_link = pyqtSignal(int)
sig_cpu = pyqtSignal(float)
sig_memory = pyqtSignal(float)
sig_temperature = pyqtSignal(float)
sig_height = pyqtSignal(str)
sig_speed = pyqtSignal(str)
sig_flight_time = pyqtSignal(str)
sig_remaining_time = pyqtSignal(str)
sig_battery_low = pyqtSignal(str)
sig_fly_mode = pyqtSignal(str)
sig_fast_mode = pyqtSignal(str)
sig_is_flying = pyqtSignal(str)
sig_remaining_time = pyqtSignal(str)
sig_foto_info = pyqtSignal(str)
sig_image = pyqtSignal(object)
# sig_mission_state = pyqtSignal(str)
def __init__(self, context):
super(TelloPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('TelloPlugin')
# 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)
# load main widget
self._widget = QWidget()
self.ui = Ui_TelloPlugin()
self.ui.setupUi(self._widget)
self.ui.tabWidget.setCurrentIndex(0)
self._widget.setObjectName('TelloPluginUi')
# Add left and right key arrow blocking
filter = KeySuppress()
self.ui.tabWidget.installEventFilter(filter)
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)
print("Tello widget added")
# Tello node
self.bridge = CvBridge()
self.pos_listener = TransformListener()
# Add recorder and Recording Timer
self.recording = False
self.record_timer = QTimer()
self.record_timer.timeout.connect(self.update_record_time)
self.record_time = QTime(00,00,00)
self.video_recorder = None
self.VIDEO_TYPE = {'avi': cv2.VideoWriter_fourcc(*'XVID'), 'mp4': cv2.VideoWriter_fourcc(*'MP4V')}
# System, process and Wifi interface info
interface = "wlp2s0"
ssid_names = "TELLO-FD143A"
self.wifi_info = WifiInfo(interface, ssid_names)
self.utility = GetUtility()
# Callbacks GUI items
self.ui.connect_button.clicked[bool].connect(self.handle_connect_clicked)
self.ui.record_button.clicked[bool].connect(self.handle_record_clicked)
self.ui.foto_button.clicked[bool].connect(self.handle_foto_clicked)
self.ui.stop_button.clicked[bool].connect(self.handle_stop_clicked)
self.ui.start_button.clicked[bool].connect(self.handle_start_clicked)
self.ui.land_button.clicked[bool].connect(self.handle_land_clicked)
self.ui.target_button.clicked[bool].connect(self.handle_target_clicked)
self.ui.calibrate_button.clicked[bool].connect(self.handle_calibrate_clicked)
# Set icons
# get an instance of RosPack with the default search paths
rospack = rospkg.RosPack()
icon_path = rospack.get_path('tello_rqt_gui') + '/src/tello_rqt/resource/icons/'
self.ui.record_button.setText('') # override text with icon
self.ui.record_button.setIcon(QIcon(QPixmap(icon_path + 'video.png')))
self.ui.record_button.setIconSize(QSize(25,25))
self.ui.foto_button.setText('') # override text with icon
self.ui.foto_button.setIcon(QIcon(QPixmap(icon_path + 'foto.png')))
self.ui.foto_button.setIconSize(QSize(25,25))
self.ui.label_record_time.setText('record')
# Set bar values to 0
self.ui.link_bar.setValue(0)
self.ui.battery_bar.setValue(0)
self.ui.cpu_bar.setValue(0)
self.ui.memory_bar.setValue(0)
self.ui.temperature_bar.setValue(0)
# create signals and slots for updates
# never access widgets and GUI related things directly from a thread other than the main thread
self.sig_connection_changed.connect(self.update_ui_state)
self.sig_update_position.connect(self.update_position_data)
self.sig_battery_percentage.connect(self.ui.battery_bar.setValue)
self.sig_tello_link.connect(self.ui.link_bar.setValue)
self.sig_wifi_sig_link.connect(self.ui.wifi_signal_bar.setValue)
self.sig_cpu.connect(self.ui.cpu_bar.setValue)
self.sig_memory.connect(self.ui.memory_bar.setValue)
self.sig_temperature.connect(self.ui.temperature_bar.setValue)
self.sig_height.connect(self.ui.label_height.setText)
self.sig_speed.connect(self.ui.label_speed.setText)
self.sig_flight_time.connect(self.ui.label_flight_time.setText)
self.sig_image.connect(self.set_image)
self.sig_battery_low.connect(self.ui.label_battery_low.setText)
self.sig_is_flying.connect(self.ui.label_is_flying.setText)
self.sig_fly_mode.connect(self.ui.label_fly_mode.setText)
self.sig_fast_mode.connect(self.ui.label_fast_mode.setText)
self.sig_remaining_time.connect(self.ui.label_remaining_time.setText)
self.sig_foto_info.connect(self.ui.label_foto.setText)
#self.sig_mission_state.connect(self.ui.label_mission_state.setText)
# Connect signals from Tello, Controller und SLAM to GUI
self.ns_tello = 'tello/'
self.ns_controller = 'tello_controller/'
self.topic_slam_pose = '/orb_slam2_mono/pose'
self.topic_slam_scale = 'tello_controller/slam_real_world_scale'
self.tello_state = self.STATE_DISCONNECTED.getname()
self.pub_connect = rospy.Publisher(self.ns_tello+'connect', Empty, queue_size=1, latch=True)
self.pub_disconnect = rospy.Publisher(self.ns_tello+'disconnect', Empty, queue_size=1, latch=True)
self.pub_take_picture = rospy.Publisher(self.ns_tello+'take_picture', Empty, queue_size=1, latch=True)
self.pub_mission_command = rospy.Publisher(self.ns_controller+'mission_command', String, queue_size=1, latch=True)
self.sub_picture_update = rospy.Subscriber(self.ns_tello+'picture_update', String, self.cb_picture_update)
self.sub_status = rospy.Subscriber(self.ns_tello+'status', TelloStatus, self.cb_status)
self.sub_connection = rospy.Subscriber(self.ns_tello+'connection_state', String, self.cb_connection)
self.sub_video = rospy.Subscriber('tello/camera/image_raw', Image, self.cb_video)
self.sub_odom = rospy.Subscriber(self.ns_tello+'odom', Odometry, self.cb_odom, queue_size=1)
self.sub_slam_pose = rospy.Subscriber(self.topic_slam_pose, PoseStamped, self.cb_slam_pose, queue_size=1)
self.sub_slam_scale = rospy.Subscriber(self.topic_slam_scale, Float32, self.cb_slam_scale, queue_size=1)
self.sub_mission = rospy.Subscriber(self.ns_controller+'mission_state', String, self.cb_mission_state)
self.cv_image = None
# create additional thread for system und process utility
self.stop_threads = False
self.thread = threading.Thread(target=self.update_sys_util, args=())
self.thread.start()
print('init done')
def update_ui_state(self):
if self.tello_state == self.STATE_QUIT.getname() or self.tello_state == self.STATE_DISCONNECTED.getname():
self.ui.connect_button.setText("Connect")
# self.ui.connect_button.setStyleSheet("background-color: None")
self.ui.link_bar.setValue(0)
self.ui.battery_bar.setValue(0)
self.ui.wifi_signal_bar.setValue(-90)
elif self.tello_state == self.STATE_CONNECTED.getname():
self.ui.connect_button.setText("Disconnect")
# self.ui.connect_button.setStyleSheet("background-color: lightgreen")
# print('end if..')
elif self.tello_state == self.STATE_CONNECTING.getname():
self.ui.connect_button.setText("Connecting...")
# Set bar values to 0
self.ui.link_bar.setValue(0)
self.ui.battery_bar.setValue(0)
self.ui.wifi_signal_bar.setValue(-90)
# self.ui.connect_button.setStyleSheet("background-color: None")
def update_sys_util(self):
# do next iteration of not shutdown
if not rospy.is_shutdown() and not self.stop_threads:
# get cpu, memory percentage and temperature
cpu, memory, temp = self.utility.get_data()
# get wifi ling quality
sig, qual = self.wifi_info.get_info()
self.sig_cpu.emit(cpu)
self.sig_memory.emit(memory)
self.sig_temperature.emit(temp)
self.sig_wifi_sig_link.emit(sig)
# start new timer
timer = 2.0 # seconds
threading.Timer(timer, self.update_sys_util).start()
def handle_connect_clicked(self):
"""
User pressed connect button
"""
print(self.tello_state)
if self.tello_state == self.STATE_CONNECTED.getname():
self.pub_disconnect.publish()
else:
self.pub_connect.publish()
def handle_record_clicked(self):
# Create a file in ~/Pictures/ to receive image data from the drone.
print('Recording: ', self.recording)
if self.recording:
# close and save video
self.recording = False
self.record_time = QTime(00,00,00)
self.ui.label_record_time.setText('record')
else:
self.record_timer.start(1000) # update every second
# self.stop_event.clear() # clear event
self.path = '%s/Videos/tello-%s.avi' % (
os.getenv('HOME'),
time.strftime("%Y-%m-%d_%H:%M:%S"))
self.video_recorder = cv2.VideoWriter(self.path, self.VIDEO_TYPE['avi'], 25, (960, 720))
self.recording = True
def handle_foto_clicked(self):
self.ui.label_foto.setText("Taking picture")
self.pub_take_picture.publish(Empty())
def handle_stop_clicked(self):
self.pub_mission_command.publish('stop')
def handle_start_clicked(self):
self.pub_mission_command.publish('start')
def handle_land_clicked(self):
self.pub_mission_command.publish('land')
def handle_target_clicked(self):
self.pub_mission_command.publish('target')
def handle_calibrate_clicked(self):
self.pub_mission_command.publish('calibrate')
def update_record_time(self):
# self.record_time = QtCore.QTime(00,00,00)
# print("Update Recording time")
if self.recording:
self.record_time = self.record_time.addSecs(1)
print(self.record_time.toString())
self.ui.label_record_time.setText(self.record_time.toString())
def update_position_data(self):
# try to look up current position
try:
(tello_trans, tello_rot) = self.pos_listener.lookupTransform('odom', 'base_link', rospy.Time(0))
# print('trans: ', tello_trans, ' | rot: ', tello_rot)
"""
euler_angles = euler_from_quaternion(tello_rot)
self.ui.label_internal_x.setText('%.2f' % tello_trans[0])
self.ui.label_internal_y.setText('%.2f' % tello_trans[1])
self.ui.label_internal_z.setText('%.2f' % tello_trans[2])
self.ui.label_internal_yaw.setText('%.2f' % euler_angles[2])
"""
except (LookupException, ConnectivityException, ExtrapolationException):
print('No Position Data available')
self.ui.label_internal_x.setText('wait')
self.ui.label_internal_y.setText('wait')
self.ui.label_internal_z.setText('wait')
self.ui.label_internal_yaw.setText('wait')
self.ui.label_slam_x.setText('wait')
self.ui.label_slam_y.setText('wait')
self.ui.label_slam_z.setText('wait')
self.ui.label_slam_yaw.setText('wait')
def cb_odom(self, msg):
current_orientation_list = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
current_angles = euler_from_quaternion(current_orientation_list)
self.ui.label_internal_x.setText('%.2f' % msg.pose.pose.position.x)
self.ui.label_internal_y.setText('%.2f' % msg.pose.pose.position.y)
self.ui.label_internal_z.setText('%.2f' % msg.pose.pose.position.z)
self.ui.label_internal_yaw.setText('%.2f' % current_angles[2])
def cb_slam_pose(self, msg):
current_orientation_list = [msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w]
current_angles = euler_from_quaternion(current_orientation_list)
self.ui.label_slam_x.setText('%.2f' % msg.pose.position.x)
self.ui.label_slam_y.setText('%.2f' % msg.pose.position.y)
self.ui.label_slam_z.setText('%.2f' % msg.pose.position.z)
self.ui.label_slam_yaw.setText('%.2f' % current_angles[2])
def cb_slam_scale(self, msg):
self.ui.label_slam_scaling.setText('%.2f' % msg.data)
def cb_picture_update(self, msg):
self.ui.label_foto.setText(msg.data)
def cb_mission_state(self, msg):
self.ui.label_mission_state.setText(msg.data)
def cb_status(self, msg):
self.sig_battery_percentage.emit(msg.battery_percentage)
self.sig_tello_link.emit(msg.wifi_strength)
self.sig_height.emit("%.2f" % msg.height_m)
self.sig_speed.emit("%.2f" % msg.speed_horizontal_mps)
self.sig_flight_time.emit("%.2f" % msg.flight_time_sec)
self.sig_battery_low.emit(str(msg.is_battery_lower))
self.sig_fly_mode.emit(str(msg.fly_mode))
self.sig_fast_mode.emit(str(msg.cmd_fast_mode))
self.sig_is_flying.emit(str(msg.is_flying))
m, s = divmod(-msg.drone_fly_time_left_sec/10., 60)
# print("Remaining: ",m,"min, ",s,"s")
self.sig_remaining_time.emit("%d:%d" % (m, s))
# check position
self.sig_update_position.emit()
def cb_connection(self, msg):
print(self.tello_state)
self.tello_state = msg.data
self.sig_connection_changed.emit()
def cb_video(self, msg):
"""
callback to record video and display video
"""
try:
self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
if self.recording:
print('recording.... ')
self.video_recorder.write(self.cv_image)
# show Video in GUI
self.sig_image.emit(self.cv_image)
def set_image(self, img_bgr):
img_rgb = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB) # switch b and r channel
# Qt Image
qt_image = QImage(img_rgb, img_rgb.shape[1], img_rgb.shape[0], QImage.Format_RGB888)
self.ui.label_video.setPixmap(QPixmap(qt_image)) # set pixmap to label
def shutdown_plugin(self):
# TODO unregister all publishers here
# wait to terminate Threads
# self._ros_process.close()
print(os.path.dirname(os.path.realpath(__file__)))
self.pub_disconnect.publish(Empty())
self.stop_threads = True
self.thread.join()
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class HistogramPlotWidget(QWidget):
_redraw_interval = 40
def __init__(self, topics):
super(HistogramPlotWidget, self).__init__()
self.setObjectName('HistogramPlotWidget')
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('jsk_rqt_plugins'), 'resource',
'plot_histogram.ui')
loadUi(ui_file, self)
self.cv_bridge = CvBridge()
self.subscribe_topic_button.setIcon(QIcon.fromTheme('add'))
self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause'))
self.clear_button.setIcon(QIcon.fromTheme('edit-clear'))
self.data_plot = MatHistogramPlot(self)
self.data_plot_layout.addWidget(self.data_plot)
self._topic_completer = TopicCompleter(self.topic_edit)
self._topic_completer.update_topics()
self.topic_edit.setCompleter(self._topic_completer)
self.data_plot.dropEvent = self.dropEvent
self.data_plot.dragEnterEvent = self.dragEnterEvent
self._start_time = rospy.get_time()
self._rosdata = None
if len(topics) != 0:
self.subscribe_topic(topics)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.update_plot)
self._update_plot_timer.start(self._redraw_interval)
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.subscribe_topic(topic_name)
@Slot()
def on_topic_edit_returnPressed(self):
if self.subscribe_topic_button.isEnabled():
self.subscribe_topic(str(self.topic_edit.text()))
@Slot()
def on_subscribe_topic_button_clicked(self):
self.subscribe_topic(str(self.topic_edit.text()))
def subscribe_topic(self, topic_name):
self.topic_with_field_name = topic_name
self.pub_image = rospy.Publisher(topic_name + "/histogram_image",
Image)
if not self._rosdata:
self._rosdata = ROSData(topic_name, self._start_time)
else:
if self._rosdata != topic_name:
self._rosdata.close()
self.data_plot.clear()
self._rosdata = ROSData(topic_name, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic_name)
def enable_timer(self, enabled=True):
if enabled:
self._update_plot_timer.start(self._redraw_interval)
else:
self._update_plot_timer.stop()
@Slot()
def on_clear_button_clicked(self):
self.data_plot.clear()
@Slot(bool)
def on_pause_button_clicked(self, checked):
self.enable_timer(not checked)
def update_plot(self):
if not self._rosdata:
return
data_x, data_y = self._rosdata.next()
if len(data_y) == 0:
return
axes = self.data_plot._canvas.axes
axes.cla()
if isinstance(data_y[-1], HistogramWithRange):
xs = [y.count for y in data_y[-1].bins]
pos = [y.min_value for y in data_y[-1].bins]
widths = [y.max_value - y.min_value for y in data_y[-1].bins]
axes.set_xlim(xmin=pos[0], xmax=pos[-1] + widths[-1])
elif isinstance(data_y[-1], collections.Sequence):
xs = data_y[-1]
pos = np.arange(len(xs))
widths = [1] * len(xs)
axes.set_xlim(xmin=0, xmax=len(xs))
else:
rospy.logerr(
"Topic/Field name '%s' has unsupported '%s' type."
"List of float values and "
"jsk_recognition_msgs/HistogramWithRange are supported." %
(self.topic_with_field_name, self._rosdata.sub.data_class))
return
#axes.xticks(range(5))
for p, x, w in zip(pos, xs, widths):
axes.bar(p, x, color='r', align='center', width=w)
axes.legend([self.topic_with_field_name], prop={'size': '8'})
self.data_plot._canvas.draw()
buffer = StringIO()
self.data_plot._canvas.figure.savefig(buffer, format="png")
buffer.seek(0)
img_array = np.asarray(bytearray(buffer.read()), dtype=np.uint8)
img = cv2.imdecode(img_array, cv2.CV_LOAD_IMAGE_COLOR)
self.pub_image.publish(self.cv_bridge.cv2_to_imgmsg(img, "bgr8"))
class TopicWidget(QWidget):
"""
main class inherits from the ui window class.
You can specify the topics that the topic pane.
TopicWidget.start must be called in order to update topic pane.
"""
SELECT_BY_NAME = 0
SELECT_BY_MSGTYPE = 1
_column_names = ['topic', 'type', 'bandwidth', 'rate', 'value']
def __init__(self, plugin=None, selected_topics=None, select_topic_type=SELECT_BY_NAME):
"""
@type selected_topics: list of tuples.
@param selected_topics: [($NAME_TOPIC$, $TYPE_TOPIC$), ...]
@type select_topic_type: int
@param select_topic_type: Can specify either the name of topics or by
the type of topic, to filter the topics to
show. If 'select_topic_type' argument is
None, this arg shouldn't be meaningful.
"""
super(TopicWidget, self).__init__()
self._select_topic_type = select_topic_type
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_topic'), 'resource', 'TopicWidget.ui')
loadUi(ui_file, self)
self._plugin = plugin
self.topics_tree_widget.sortByColumn(0, Qt.AscendingOrder)
header = self.topics_tree_widget.header()
header.setResizeMode(QHeaderView.ResizeToContents)
header.customContextMenuRequested.connect(self.handle_header_view_customContextMenuRequested)
header.setContextMenuPolicy(Qt.CustomContextMenu)
# Whether to get all topics or only the topics that are set in advance.
# Can be also set by the setter method "set_selected_topics".
self._selected_topics = selected_topics
self._current_topic_list = []
self._topics = {}
self._tree_items = {}
self._column_index = {}
for column_name in self._column_names:
self._column_index[column_name] = len(self._column_index)
# self.refresh_topics()
# init and start update timer
self._timer_refresh_topics = QTimer(self)
self._timer_refresh_topics.timeout.connect(self.refresh_topics)
def set_topic_specifier(self, specifier):
self._select_topic_type = specifier
def start(self):
"""
This method needs to be called to start updating topic pane.
"""
self._timer_refresh_topics.start(1000)
@Slot()
def refresh_topics(self):
"""
refresh tree view items
"""
if self._selected_topics is None:
topic_list = rospy.get_published_topics()
if topic_list is None:
rospy.logerr('Not even a single published topic found. Check network configuration')
return
else: # Topics to show are specified.
topic_list = self._selected_topics
topic_specifiers_server_all = None
topic_specifiers_required = None
rospy.logdebug('refresh_topics) self._selected_topics=%s' % (topic_list,))
if self._select_topic_type == self.SELECT_BY_NAME:
topic_specifiers_server_all = [name for name, type in rospy.get_published_topics()]
topic_specifiers_required = [name for name, type in topic_list]
elif self._select_topic_type == self.SELECT_BY_MSGTYPE:
# The topics that are required (by whoever uses this class).
topic_specifiers_required = [type for name, type in topic_list]
# The required topics that match with published topics.
topics_match = [(name, type) for name, type in rospy.get_published_topics() if type in topic_specifiers_required]
topic_list = topics_match
rospy.logdebug('selected & published topic types=%s' % (topic_list,))
rospy.logdebug('server_all=%s\nrequired=%s\ntlist=%s' % (topic_specifiers_server_all, topic_specifiers_required, topic_list))
if len(topic_list) == 0:
rospy.logerr('None of the following required topics are found.\n(NAME, TYPE): %s' % (self._selected_topics,))
return
if self._current_topic_list != topic_list:
self._current_topic_list = topic_list
# start new topic dict
new_topics = {}
for topic_name, topic_type in topic_list:
# if topic is new or has changed its type
if topic_name not in self._topics or \
self._topics[topic_name]['type'] != topic_type:
# create new TopicInfo
topic_info = TopicInfo(topic_name, topic_type)
message_instance = None
if topic_info.message_class is not None:
message_instance = topic_info.message_class()
# add it to the dict and tree view
topic_item = self._recursive_create_widget_items(self.topics_tree_widget, topic_name, topic_type, message_instance)
new_topics[topic_name] = {
'item': topic_item,
'info': topic_info,
'type': topic_type,
}
else:
# if topic has been seen before, copy it to new dict and
# remove it from the old one
new_topics[topic_name] = self._topics[topic_name]
del self._topics[topic_name]
# clean up old topics
for topic_name in self._topics.keys():
self._topics[topic_name]['info'].stop_monitoring()
index = self.topics_tree_widget.indexOfTopLevelItem(
self._topics[topic_name]['item'])
self.topics_tree_widget.takeTopLevelItem(index)
del self._topics[topic_name]
# switch to new topic dict
self._topics = new_topics
self._update_topics_data()
def _update_topics_data(self):
for topic in self._topics.values():
topic_info = topic['info']
if topic_info.monitoring:
# update rate
rate, _, _, _ = topic_info.get_hz()
rate_text = '%1.2f' % rate if rate != None else 'unknown'
# update bandwidth
bytes_per_s, _, _, _ = topic_info.get_bw()
if bytes_per_s is None:
bandwidth_text = 'unknown'
elif bytes_per_s < 1000:
bandwidth_text = '%.2fB/s' % bytes_per_s
elif bytes_per_s < 1000000:
bandwidth_text = '%.2fKB/s' % (bytes_per_s / 1000.)
else:
bandwidth_text = '%.2fMB/s' % (bytes_per_s / 1000000.)
# update values
value_text = ''
self.update_value(topic_info._topic_name, topic_info.last_message)
else:
rate_text = ''
bandwidth_text = ''
value_text = 'not monitored' if topic_info.error is None else topic_info.error
self._tree_items[topic_info._topic_name].setText(self._column_index['rate'], rate_text)
self._tree_items[topic_info._topic_name].setText(self._column_index['bandwidth'], bandwidth_text)
self._tree_items[topic_info._topic_name].setText(self._column_index['value'], value_text)
def update_value(self, topic_name, message):
if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
for slot_name in message.__slots__:
self.update_value(topic_name + '/' + slot_name, getattr(message, slot_name))
elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(message[0], '__slots__'):
for index, slot in enumerate(message):
if topic_name + '[%d]' % index in self._tree_items:
self.update_value(topic_name + '[%d]' % index, slot)
else:
base_type_str, _ = self._extract_array_info(self._tree_items[topic_name].text(self._column_index['type']))
self._recursive_create_widget_items(self._tree_items[topic_name], topic_name + '[%d]' % index, base_type_str, slot)
# remove obsolete children
if len(message) < self._tree_items[topic_name].childCount():
for i in range(len(message), self._tree_items[topic_name].childCount()):
item_topic_name = topic_name + '[%d]' % i
self._recursive_delete_widget_items(self._tree_items[item_topic_name])
else:
if topic_name in self._tree_items:
self._tree_items[topic_name].setText(self._column_index['value'], repr(message))
def _extract_array_info(self, type_str):
array_size = None
if '[' in type_str and type_str[-1] == ']':
type_str, array_size_str = type_str.split('[', 1)
array_size_str = array_size_str[:-1]
if len(array_size_str) > 0:
array_size = int(array_size_str)
else:
array_size = 0
return type_str, array_size
def _recursive_create_widget_items(self, parent, topic_name, type_name, message):
if parent is self.topics_tree_widget:
# show full topic name with preceding namespace on toplevel item
topic_text = topic_name
item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent)
else:
topic_text = topic_name.split('/')[-1]
if '[' in topic_text:
topic_text = topic_text[topic_text.index('['):]
item = QTreeWidgetItem(parent)
item.setText(self._column_index['topic'], topic_text)
item.setText(self._column_index['type'], type_name)
item.setData(0, Qt.UserRole, topic_name)
self._tree_items[topic_name] = item
if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
for slot_name, type_name in zip(message.__slots__, message._slot_types):
self._recursive_create_widget_items(item, topic_name + '/' + slot_name, type_name, getattr(message, slot_name))
else:
base_type_str, array_size = self._extract_array_info(type_name)
try:
base_instance = roslib.message.get_message_class(base_type_str)()
except (ValueError, TypeError):
base_instance = None
if array_size is not None and hasattr(base_instance, '__slots__'):
for index in range(array_size):
self._recursive_create_widget_items(item, topic_name + '[%d]' % index, base_type_str, base_instance)
return item
def _toggle_monitoring(self, topic_name):
item = self._tree_items[topic_name]
if item.checkState(0):
self._topics[topic_name]['info'].start_monitoring()
else:
self._topics[topic_name]['info'].stop_monitoring()
def _recursive_delete_widget_items(self, item):
def _recursive_remove_items_from_tree(item):
for index in reversed(range(item.childCount())):
_recursive_remove_items_from_tree(item.child(index))
topic_name = item.data(0, Qt.UserRole)
del self._tree_items[topic_name]
_recursive_remove_items_from_tree(item)
item.parent().removeChild(item)
@Slot('QPoint')
def handle_header_view_customContextMenuRequested(self, pos):
header = self.topics_tree_widget.header()
# show context menu
menu = QMenu(self)
action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize')
action = menu.exec_(header.mapToGlobal(pos))
# evaluate user action
if action is action_toggle_auto_resize:
if header.resizeMode(0) == QHeaderView.ResizeToContents:
header.setResizeMode(QHeaderView.Interactive)
else:
header.setResizeMode(QHeaderView.ResizeToContents)
@Slot('QPoint')
def on_topics_tree_widget_customContextMenuRequested(self, pos):
item = self.topics_tree_widget.itemAt(pos)
if item is None:
return
# show context menu
menu = QMenu(self)
action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'), 'Expand All Children')
action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'), 'Collapse All Children')
action = menu.exec_(self.topics_tree_widget.mapToGlobal(pos))
# evaluate user action
if action in (action_item_expand, action_item_collapse):
expanded = (action is action_item_expand)
def recursive_set_expanded(item):
item.setExpanded(expanded)
for index in range(item.childCount()):
recursive_set_expanded(item.child(index))
recursive_set_expanded(item)
def shutdown_plugin(self):
for topic in self._topics.values():
topic['info'].stop_monitoring()
self._timer_refresh_topics.stop()
def set_selected_topics(self, selected_topics):
"""
@param selected_topics: list of tuple. [(topic_name, topic_type)]
@type selected_topics: []
"""
rospy.logdebug('set_selected_topics topics={}'.format(
len(selected_topics)))
self._selected_topics = selected_topics
class Console(Plugin):
"""
rqt_console plugin's main class. Handles communication with ros_gui and contains
callbacks to handle incoming message
"""
def __init__(self, context):
"""
:param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext''
"""
super(Console, self).__init__(context)
self.setObjectName('Console')
self._datamodel = MessageDataModel()
self._proxymodel = MessageProxyModel()
self._proxymodel.setSourceModel(self._datamodel)
self._mainwindow = ConsoleWidget(self._proxymodel)
if context.serial_number() > 1:
self._mainwindow.setWindowTitle(self._mainwindow.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._mainwindow)
self._consolesubscriber = ConsoleSubscriber(self.message_callback)
# Timer and Mutex for flushing recieved messages to datamodel.
# Required since QSortProxyModel can not handle a high insert rate
self._mutex = QMutex()
self._timer = QTimer()
self._timer.timeout.connect(self.insert_messages)
self._timer.start(100)
def insert_messages(self):
"""
Callback for flushing incoming Log messages from the queue to the datamodel
"""
self._mutex.lock()
msgs = self._datamodel._insert_message_queue
self._datamodel._insert_message_queue = []
self._mutex.unlock()
self._datamodel.insert_rows(msgs)
self._mainwindow.update_status()
def message_callback(self, msg):
"""
Callback for adding an incomming message to the queue
"""
if not self._datamodel._paused:
self._mutex.lock()
self._datamodel._insert_message_queue.append(msg)
self._mutex.unlock()
def shutdown_plugin(self):
self._consolesubscriber.unsubscribe_topic()
self._timer.stop()
self._mainwindow.cleanup_browsers_on_close()
def save_settings(self, plugin_settings, instance_settings):
self._mainwindow.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._mainwindow.restore_settings(plugin_settings, instance_settings)
def trigger_configuration(self):
self._consolesubscriber.set_message_limit(self._datamodel._message_limit)
ok = self._consolesubscriber.show_dialog()
if ok:
self._datamodel._message_limit = self._consolesubscriber.get_message_limit()
class BarrettDashboard(Plugin):
def __init__(self, context):
super(BarrettDashboard, self).__init__(context)
self._joint_sub = None
# Give QObjects reasonable names
self.setObjectName('BarrettDashboard')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'barrett_dashboard.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('BarrettDashboardPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.context = context
jp_widgets = [getattr(self._widget,'jp_%d' % i) for i in range(7)]
jn_widgets = [getattr(self._widget,'jn_%d' % i) for i in range(7)]
self.joint_widgets = zip(jp_widgets,jn_widgets)
tp_widgets = [getattr(self._widget,'tp_%d' % i) for i in range(7)]
tn_widgets = [getattr(self._widget,'tn_%d' % i) for i in range(7)]
self.torque_widgets = zip(tp_widgets,tn_widgets)
self.joint_signals = []
self.torque_signals = []
for (tp,tn) in self.torque_widgets:
tp.setRange(0.0,1.0,False)
tn.setRange(1.0,0.0,False)
tp.setValue(0.0)
tn.setValue(0.0)
# set random values for testing
#v = (2.0*random.random()) - 1.0
#tp.setValue(v if v >=0 else 0)
#tn.setValue(-v if v <0 else 0)
for (jp,jn) in self.joint_widgets:
jp.setRange(0.0,1.0,False)
jn.setRange(1.0,0.0,False)
jp.setValue(0.0)
jn.setValue(0.0)
# set random values for testing
#v = (2.0*random.random()) - 1.0
#jp.setValue(v if v >=0 else 0)
#jn.setValue(-v if v <0 else 0)
self.barrett_green = QColor(87,186,142)
self.barrett_green_dark = self.barrett_green.darker()
self.barrett_green_light = self.barrett_green.lighter()
self.barrett_blue = QColor(80,148,204)
self.barrett_blue_dark = self.barrett_blue.darker()
self.barrett_blue_light = self.barrett_blue.lighter()
self.barrett_red = QColor(232,47,47)
self.barrett_red_dark = self.barrett_red.darker()
self.barrett_red_light = self.barrett_red.lighter()
self.barrett_orange = QColor(255,103,43)
self.barrett_orange_dark = self.barrett_orange.darker()
#background_color = p.mid().color()
joint_bg_color = self.barrett_blue_dark.darker()
joint_fill_color = self.barrett_blue
joint_alarm_color = self.barrett_blue #self.barrett_blue_light
torque_bg_color = self.barrett_green_dark.darker()
torque_fill_color = self.barrett_green
torque_alarm_color = self.barrett_orange #self.barrett_green_light
temp_bg_color = self.barrett_red_dark.darker()
temp_fill_color = self.barrett_orange
temp_alarm_color = self.barrett_red
for w in jp_widgets + jn_widgets:
w.setAlarmLevel(0.80)
w.setFillColor(joint_fill_color)
w.setAlarmColor(joint_alarm_color)
p = w.palette()
p.setColor(tp.backgroundRole(), joint_bg_color)
w.setPalette(p)
for w in tp_widgets + tn_widgets:
w.setAlarmLevel(0.66)
w.setFillColor(torque_fill_color)
w.setAlarmColor(torque_alarm_color)
p = w.palette()
p.setColor(tp.backgroundRole(), torque_bg_color)
w.setPalette(p)
self._widget.hand_temp.setAlarmLevel(0.66)
self._widget.hand_temp.setFillColor(temp_fill_color)
self._widget.hand_temp.setAlarmColor(temp_alarm_color)
p = self._widget.hand_temp.palette()
p.setColor(self._widget.hand_temp.backgroundRole(), temp_bg_color)
self._widget.hand_temp.setPalette(p)
self._widget.hand_temp.setOrientation(Qt.Horizontal, QwtThermo.RightScale)
self._widget.jf_0.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % (
joint_bg_color.red(), joint_bg_color.green(), joint_bg_color.blue(), joint_fill_color.red(), joint_fill_color.green(), joint_fill_color.blue()))
self.urdf = rospy.get_param('robot_description')
self.robot = URDF()
self.robot = self.robot.from_xml_string(self.urdf)
self.pos_norm = [0] * 7
self.torque_norm = [0] * 7
self._joint_sub = rospy.Subscriber(
'joint_states',
sensor_msgs.msg.JointState,
self._joint_state_cb)
self._status_sub = rospy.Subscriber(
'status',
oro_barrett_msgs.msg.BarrettStatus,
self._status_cb)
self._hand_status_sub = rospy.Subscriber(
'hand/status',
oro_barrett_msgs.msg.BHandStatus,
self._hand_status_cb)
self._update_status(-1)
self.safety_mode = -1
self.run_mode = 0
self.hand_run_mode = 0
self.hand_min_temperature = 40.0
self.hand_max_temperature = 65.0
self.hand_temperature = 0.0
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widget_values)
self.update_timer.start()
self.set_home_client = actionlib.SimpleActionClient(
'wam/set_home_action',
oro_barrett_msgs.msg.SetHomeAction)
self.set_mode_client = actionlib.SimpleActionClient(
'wam/set_mode_action',
oro_barrett_msgs.msg.SetModeAction)
self._widget.button_set_home.clicked[bool].connect(self._handle_set_home_clicked)
self._widget.button_idle_wam.clicked[bool].connect(self._handle_idle_wam_clicked)
self._widget.button_run_wam.clicked[bool].connect(self._handle_run_wam_clicked)
self.bhand_init_client = actionlib.SimpleActionClient(
'hand/initialize_action',
oro_barrett_msgs.msg.BHandInitAction)
self.bhand_set_mode_client = actionlib.SimpleActionClient(
'hand/set_mode_action',
oro_barrett_msgs.msg.BHandSetModeAction)
self.grasp_client = actionlib.SimpleActionClient(
'grasp', BHandGraspAction)
self.release_client = actionlib.SimpleActionClient(
'release', BHandReleaseAction)
self.spread_client = actionlib.SimpleActionClient(
'spread', BHandSpreadAction)
self._widget.button_initialize_hand.clicked[bool].connect(self._handle_initialize_hand_clicked)
self._widget.button_idle_hand.clicked[bool].connect(self._handle_idle_hand_clicked)
self._widget.button_run_hand.clicked[bool].connect(self._handle_run_hand_clicked)
self._widget.button_release_hand.clicked[bool].connect(self._handle_release_hand_clicked)
self._widget.button_grasp_hand.clicked[bool].connect(self._handle_grasp_hand_clicked)
self._widget.button_set_spread.clicked[bool].connect(self._handle_spread_hand_clicked)
self._widget.resizeEvent = self._handle_resize
def _handle_resize(self, event):
for i,((w1,w2),(w3,w4)) in enumerate(zip(self.joint_widgets, self.torque_widgets)):
width = 0.8*getattr(self._widget, 'jcc_%d'%i).contentsRect().width()
w1.setPipeWidth(width)
w2.setPipeWidth(width)
w3.setPipeWidth(width)
w4.setPipeWidth(width)
def _handle_set_home_clicked(self, checked):
if checked:
self.set_home()
def _update_status(self, status):
if status == -1:
self._widget.safety_mode.setText('UNKNOWN SAFETY MODE')
self._widget.safety_mode.setToolTip('The WAM is in an unknown state. Proceed with caution.')
color = QColor(200,200,200)
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
elif status == 0:
self._widget.safety_mode.setText('E-STOP')
self._widget.safety_mode.setToolTip('The WAM is stopped and unpowered. Determine the source of the fault and reset the arm to continue using it.')
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_red
else:
if not self.homed:
self._widget.safety_mode.setText('UNCALIBRATED')
self._widget.safety_mode.setToolTip('The WAM is not calibrated. Please place it in the calibration posture and click the "Calibrate" button.')
self._widget.button_set_home.setEnabled(True)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_orange
else:
if status == 1:
self._widget.safety_mode.setText('IDLE')
self._widget.safety_mode.setToolTip('The WAM is running but all joints are passive. It is safe to home the arm.')
self._widget.button_set_home.setEnabled(True)
self._widget.button_idle_wam.setEnabled(True)
self._widget.button_run_wam.setEnabled(True)
color = self.barrett_blue
elif status == 2:
self._widget.safety_mode.setText('ACTIVE')
self._widget.safety_mode.setToolTip('The WAM is running and all joints are active. Proceed with caution.')
self._widget.button_set_home.setEnabled(False)
self._widget.button_idle_wam.setEnabled(False)
self._widget.button_run_wam.setEnabled(False)
color = self.barrett_green
darker = color.darker()
lighter = color.lighter()
self._widget.safety_mode.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % (
color.red(), color.green(), color.blue(), lighter.red(), lighter.green(), lighter.blue()))
def _update_widget_values(self):
if self.safety_mode in (oro_barrett_msgs.msg.SafetyMode.ACTIVE, oro_barrett_msgs.msg.SafetyMode.IDLE):
for (v,(tp,tn)) in zip(self.torque_norm,self.torque_widgets):
tp.setEnabled(True)
tn.setEnabled(True)
tp.setValue(v if v >=0 else 0)
tn.setValue(-v if v <0 else 0)
for (v,(jp,jn)) in zip(self.pos_norm,self.joint_widgets):
jp.setEnabled(True)
jn.setEnabled(True)
jp.setValue(v if v >=0 else 0)
jn.setValue(-v if v <0 else 0)
else:
for (p,n) in self.joint_widgets + self.torque_widgets:
p.setEnabled(True)
n.setEnabled(True)
self._widget.hand_temp.setValue((self.hand_temperature-self.hand_min_temperature)/(self.hand_max_temperature-self.hand_min_temperature))
self._update_status(self.safety_mode)
self._update_buttons(self.run_mode, self.hand_run_mode)
def _update_buttons(self, run_mode, hand_run_mode):
if run_mode == oro_barrett_msgs.msg.RunMode.IDLE:
self._widget.button_idle_wam.setChecked(True)
self._widget.button_run_wam.setChecked(False)
else:
self._widget.button_idle_wam.setChecked(False)
self._widget.button_run_wam.setChecked(True)
if hand_run_mode == oro_barrett_msgs.msg.RunMode.RUN:
self._widget.button_idle_hand.setChecked(False)
self._widget.button_run_hand.setChecked(True)
else:
self._widget.button_idle_hand.setChecked(True)
self._widget.button_run_hand.setChecked(False)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
#def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def _joint_state_cb(self, msg):
joint_pos_map = dict(zip(msg.name, msg.position))
for (name,pos,torque,i) in zip(msg.name,msg.position,msg.effort,range(7)):
joint = self.robot.joint_map[name]
self.pos_norm[i] = 2.0*((pos-joint.limit.lower)/(joint.limit.upper-joint.limit.lower)) - 1.0
self.torque_norm[i] = torque/joint.limit.effort
def _status_cb(self, msg):
self.safety_mode = msg.safety_mode.value
self.run_mode = msg.run_mode.value
self.homed = msg.homed
if self.safety_mode == oro_barrett_msgs.msg.SafetyMode.ACTIVE:
self._widget.button_initialize_hand.setEnabled(False)
elif self.safety_mode == oro_barrett_msgs.msg.SafetyMode.ESTOP:
self._widget.button_initialize_hand.setEnabled(False)
self._widget.button_idle_hand.setEnabled(False)
self._widget.button_run_hand.setEnabled(False)
def _hand_status_cb(self, msg):
self.hand_initialized = msg.initialized
self.hand_temperature = max(msg.temperature)
self.hand_run_mode = msg.run_mode.value
def _handle_set_home_clicked(self, checked):
goal = oro_barrett_msgs.msg.SetHomeGoal()
self.set_home_client.send_goal(goal)
def _handle_idle_wam_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.SetModeGoal()
goal.mode.value = oro_barrett_msgs.msg.RunMode.IDLE
self.set_mode_client.send_goal(goal)
def _handle_run_wam_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.SetModeGoal()
goal.mode.value = oro_barrett_msgs.msg.RunMode.RUN
self.set_mode_client.send_goal(goal)
def _handle_initialize_hand_clicked(self, checked):
if self.safety_mode == oro_barrett_msgs.msg.SafetyMode.IDLE:
goal = oro_barrett_msgs.msg.BHandInitGoal()
self.bhand_init_client.send_goal(goal)
def _handle_run_hand_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.BHandSetModeGoal()
goal.run_mode.value = oro_barrett_msgs.msg.RunMode.RUN
self.bhand_set_mode_client.send_goal(goal)
def _handle_idle_hand_clicked(self, checked):
if checked:
goal = oro_barrett_msgs.msg.BHandSetModeGoal()
goal.run_mode.value = oro_barrett_msgs.msg.RunMode.IDLE
self.bhand_set_mode_client.send_goal(goal)
def _get_grasp_mask(self):
return [ self._widget.button_use_f1.isChecked(),
self._widget.button_use_f2.isChecked(),
self._widget.button_use_f3.isChecked()]
def _handle_release_hand_clicked(self, checked):
goal = BHandReleaseGoal()
goal.release_mask = self._get_grasp_mask()
goal.release_speed = [3.0, 3.0, 3.0]
self.release_client.send_goal(goal)
def _handle_grasp_hand_clicked(self, checked):
goal = BHandGraspGoal()
goal.grasp_mask = self._get_grasp_mask()
goal.grasp_speed = [1.0, 1.0, 1.0]
goal.grasp_effort = [1.0, 1.0, 1.0]
goal.min_fingertip_radius = 0.0
self.grasp_client.send_goal(goal)
def _handle_spread_hand_clicked(self, checked):
goal = BHandSpreadGoal()
goal.spread_position = self._widget.spread_slider.value()/1000.0*3.141
self.spread_client.send_goal(goal)
class JointControlPlugin(Plugin):
CM_UPDATE_FREQ = 1
CONTROLLERS = {
'position_controllers/JointPositionController':
JointPositionControllerWidget,
}
def __init__(self, context):
super(JointControlPlugin, self).__init__(context)
self.setObjectName('JointControlPlugin')
self._widget = loadUi('JointControl.ui')
self._widget.setObjectName('JointControlPluginUi')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self._widget.robot_descriptions_list.currentIndexChanged[str].connect(
self._on_robot_descriptions_list_change)
self._widget.cm_list.currentIndexChanged[str].connect(
self._on_cm_change)
self._urdf = None
self._cm_ns = None
self._controllers = []
self._update_robot_descriptions()
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 / self.CM_UPDATE_FREQ)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
self._list_controllers = lambda: []
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
def _update_robot_descriptions(self):
update_combo(self._widget.robot_descriptions_list,
get_robot_description_ns_list())
def _update_cm_list(self):
update_combo(self._widget.cm_list, self._list_cm())
def _on_robot_descriptions_list_change(self, robot_description_ns):
desc = rosparam.get_param(robot_description_ns)
try:
xml = etree.fromstring(desc)
self._urdf = xml.xpath('/robot')[0]
except:
traceback.print_exc()
self._urdf = None
self._update_controllers_list()
def _on_cm_change(self, cm_ns):
self._cm_ns = cm_ns
self._list_controllers = ControllerLister(cm_ns) if cm_ns else (
lambda: [])
self._update_controllers_list()
def _update_controllers_list(self):
controllers = self._list_controllers()
if controllers == self._controllers:
return
for _ in xrange(self._widget.controllers.count()):
self._widget.controllers.takeAt(0)
self._controllers = controllers
joint_state_controller = find(
lambda c: c.type == 'joint_state_controller/JointStateController',
controllers)
for controller in controllers:
self._add_controller(controller)
def _add_controller(self, controller):
if controller.type not in self.CONTROLLERS:
return
widget = self.CONTROLLERS[controller.type](self._cm_ns, controller,
self._urdf)
self._widget.controllers.addWidget(widget)
class MapDrawer():
def __init__(self, mainWidget):
self.mainWidget = mainWidget
# create GL view
self._gl_view = mainWidget._gl_view
self._gl_view.setAcceptDrops(True)
self.resolution_meters = 50
self._cross = None
# backup and replace original paint method
self._gl_view.paintGL_original = self._gl_view.paintGL
self._gl_view.paintGL = self._gl_view_paintGL
self._grid_layer = GridLayer(self.resolution_meters,self._gl_view)
self._submarine_layer = SubmarineLayer(self.resolution_meters,self._gl_view)
self._coor_layer = CoorSystemLayer(self._gl_view)
self._target_layer = TargetLayer(self._gl_view)
self._path_layer = PathLayer(self.resolution_meters,self._gl_view)
self._layers = []
self._layers.append(self._grid_layer)
self._layers.append(self._submarine_layer)
self._layers.append(self._coor_layer)
self._layers.append(self._target_layer)
self._layers.append(self._path_layer)
# backup and replace original mouse release method
self._gl_view.mouseReleaseEvent_original = self._gl_view.mouseReleaseEvent
self._gl_view.mouseReleaseEvent = self.mainWidget._gl_view_mouseReleaseEvent
# init and start update timer with 40ms (25fps)
self._update_timer = QTimer(mainWidget)
self._update_timer.timeout.connect(self.update_timeout)
self._update_timer.start(100)
def get_layers(self):
return self._layers
def set_position(self,position):
self._submarine_layer.set_position(position)
self._path_layer.position_update(position)
def set_orientation(self,orientation,yaw):
self._submarine_layer.set_orientation(orientation,yaw)
def _set_default_view(self):
self._gl_view.makeCurrent()
self._gl_view.reset_view()
#self._gl_view.rotate((0, 0, 1), 45)
#self._gl_view.rotate((1, 0, 0), -65)
self._gl_view.translate((0, 0, -800))
def update_timeout(self):
self._gl_view.makeCurrent()
self._gl_view.updateGL()
def _gl_view_paintGL(self):
self._gl_view.paintGL_original()
self._grid_layer.draw()
self._coor_layer.draw()
self._target_layer.draw()
self._path_layer.draw()
self._submarine_layer.draw()
def reset_path(self):
self._path_layer.reset_path()
def drawTarget(self, x, y, z):
self._target_layer.set_target((x * self.resolution_meters, y * self.resolution_meters, z * self.resolution_meters))
def set_lock_on_sub_activated(self,activate):
self._submarine_layer.set_lock_on_sub(activate)
def set_rotate_with_sub_activated(self,activate):
self._submarine_layer.set_rotate_with_sub_activated(activate)
def save_settings(self, plugin_settings, instance_settings):
for layer in self._layers:
layer.save_settings(plugin_settings,instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
for layer in self._layers:
layer.restore_settings(plugin_settings,instance_settings)
def shutdown_plugin(self):
print 'Shutting down'
class Control(Plugin):
def __init__(self, context):
super(Control, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Control')
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of
# this package
ui_file = os.path.join(rospkg.RosPack().get_path('robosub'),
'src/rqt/rqt_control/resource', 'Control.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
self.control_timer = QTimer(self)
self.control_timer.timeout.connect(self.control_missed)
self.control_timer.start(1000)
self.control_status_timer = QTimer(self)
self.control_status_timer.timeout.connect(self.control_status_missed)
self.control_status_timer.start(1000)
# Give QObjects reasonable names
self._widget.setObjectName('Control')
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._widget.statusActive.hide()
self._widget.controlActive.hide()
self.con_sub = rospy.Subscriber('control', control,
self.control_callback, queue_size=1)
self.cs_sub = rospy.Subscriber('control_status', control_status,
self.control_status_callback,
queue_size=1)
img_file = os.path.join(rospkg.RosPack().get_path('robosub'),
'src/rqt/resource/robosub_logo.png')
self._widget.setStyleSheet(".QWidget {background-image: url(" +
img_file +
"); background-repeat: no-repeat;" +
"background-position:bottom right}")
def control_missed(self):
if not self._widget.controlStale.isVisible():
self._widget.controlStale.show()
self._widget.controlActive.hide()
def control_status_missed(self):
if not self._widget.statusStale.isVisible():
self._widget.statusStale.show()
self._widget.statusActive.hide()
def control_status_callback(self, m):
try:
self.control_status_timer.stop()
except RuntimeError:
pass
if self._widget.statusStale.isVisible():
self._widget.statusStale.setVisible(False)
self._widget.statusActive.setVisible(True)
# Set the states
self._widget.forwardStatusState.setText(m.forward_state)
self._widget.strafeStatusState.setText(m.strafe_left_state)
self._widget.diveStatusState.setText(m.dive_state)
self._widget.rollStatusState.setText(m.roll_right_state)
self._widget.pitchStatusState.setText(m.pitch_down_state)
self._widget.yawStatusState.setText(m.yaw_left_state)
self._widget.forwardGoal.setText("{:.4f}".format(m.forward_goal))
self._widget.strafeGoal.setText("{:.4f}".format(m.strafe_left_goal))
self._widget.diveGoal.setText("{:.4f}".format(m.dive_goal))
self._widget.rollGoal.setText("{:.4f}".format(m.roll_right_goal))
self._widget.pitchGoal.setText("{:.4f}".format(m.pitch_down_goal))
self._widget.yawGoal.setText("{:.4f}".format(m.yaw_left_goal))
self.control_status_timer.start(1000)
def control_callback(self, m):
try:
self.control_timer.stop()
except RuntimeError:
pass
if self._widget.controlStale.isVisible():
self._widget.controlStale.hide()
self._widget.controlActive.show()
# Set the states
self._widget.forwardState.setText(state_types[m.forward_state])
self._widget.strafeState.setText(state_types[m.strafe_state])
self._widget.diveState.setText(state_types[m.dive_state])
self._widget.rollState.setText(state_types[m.roll_state])
self._widget.pitchState.setText(state_types[m.pitch_state])
self._widget.yawState.setText(state_types[m.yaw_state])
self._widget.forwardValue.setText("{:.4f}".format(m.forward))
self._widget.strafeValue.setText("{:.4f}".format(m.strafe_left))
self._widget.diveValue.setText("{:.4f}".format(m.dive))
self._widget.rollValue.setText("{:.4f}".format(m.roll_right))
self._widget.pitchValue.setText("{:.4f}".format(m.pitch_down))
self._widget.yawValue.setText("{:.4f}".format(m.yaw_left))
self.control_timer.start(1000)
def shutdown_plugin(self):
self.cs_sub.unregister()
self.con_sub.unregister()
self.control_timer.stop()
self.control_status_timer.stop()
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 main(self, argv=None, standalone=None, plugin_argument_provider=None, plugin_manager_settings_prefix=''):
if argv is None:
argv = sys.argv
# extract --args and everything behind manually since argparse can not handle that
arguments = argv[1:]
# extract plugin specific args when not being invoked in standalone mode programmatically
if not standalone:
plugin_args = []
if '--args' in arguments:
index = arguments.index('--args')
plugin_args = arguments[index + 1:]
arguments = arguments[0:index + 1]
parser = ArgumentParser(os.path.basename(Main.main_filename), add_help=False)
self.add_arguments(parser, standalone=bool(standalone), plugin_argument_provider=plugin_argument_provider)
self._options = parser.parse_args(arguments)
if standalone:
# rerun parsing to separate common arguments from plugin specific arguments
parser = ArgumentParser(os.path.basename(Main.main_filename), add_help=False)
self.add_arguments(parser, standalone=bool(standalone))
self._options, plugin_args = parser.parse_known_args(arguments)
self._options.plugin_args = plugin_args
# set default values for options not available in standalone mode
if standalone:
self._options.freeze_layout = False
self._options.lock_perspective = False
self._options.multi_process = False
self._options.perspective = None
self._options.perspective_file = None
self._options.standalone_plugin = standalone
self._options.list_perspectives = False
self._options.list_plugins = False
self._options.command_pid = None
self._options.command_start_plugin = None
self._options.command_switch_perspective = None
self._options.embed_plugin = None
self._options.embed_plugin_serial = None
self._options.embed_plugin_address = None
# check option dependencies
try:
if self._options.plugin_args and not self._options.standalone_plugin and not self._options.command_start_plugin and not self._options.embed_plugin:
raise RuntimeError('Option --args can only be used together with either --standalone, --command-start-plugin or --embed-plugin option')
if self._options.freeze_layout and not self._options.lock_perspective:
raise RuntimeError('Option --freeze_layout can only be used together with the --lock_perspective option')
list_options = (self._options.list_perspectives, self._options.list_plugins)
list_options_set = [opt for opt in list_options if opt is not False]
if len(list_options_set) > 1:
raise RuntimeError('Only one --list-* option can be used at a time')
command_options = (self._options.command_start_plugin, self._options.command_switch_perspective)
command_options_set = [opt for opt in command_options if opt is not None]
if len(command_options_set) > 0 and not self._dbus_available:
raise RuntimeError('Without DBus support the --command-* options are not available')
if len(command_options_set) > 1:
raise RuntimeError('Only one --command-* option can be used at a time (except --command-pid which is optional)')
if len(command_options_set) == 0 and self._options.command_pid is not None:
raise RuntimeError('Option --command_pid can only be used together with an other --command-* option')
embed_options = (self._options.embed_plugin, self._options.embed_plugin_serial, self._options.embed_plugin_address)
embed_options_set = [opt for opt in embed_options if opt is not None]
if len(command_options_set) > 0 and not self._dbus_available:
raise RuntimeError('Without DBus support the --embed-* options are not available')
if len(embed_options_set) > 0 and len(embed_options_set) < len(embed_options):
raise RuntimeError('Missing option(s) - all \'--embed-*\' options must be set')
if len(embed_options_set) > 0 and self._options.clear_config:
raise RuntimeError('Option --clear-config can only be used without any --embed-* option')
groups = (list_options_set, command_options_set, embed_options_set)
groups_set = [opt for opt in groups if len(opt) > 0]
if len(groups_set) > 1:
raise RuntimeError('Options from different groups (--list, --command, --embed) can not be used together')
perspective_options = (self._options.perspective, self._options.perspective_file)
perspective_options_set = [opt for opt in perspective_options if opt is not None]
if len(perspective_options_set) > 1:
raise RuntimeError('Only one --perspective-* option can be used at a time')
if self._options.perspective_file is not None and not os.path.isfile(self._options.perspective_file):
raise RuntimeError('Option --perspective-file must reference existing file')
except RuntimeError as e:
print(str(e))
#parser.parse_args(['--help'])
# calling --help will exit
return 1
# set implicit option dependencies
if self._options.standalone_plugin is not None:
self._options.lock_perspective = True
# create application context containing various relevant information
from .application_context import ApplicationContext
context = ApplicationContext()
context.qtgui_path = self._qtgui_path
context.options = self._options
if self._dbus_available:
from dbus import DBusException, Interface, SessionBus
# non-special applications provide various dbus interfaces
if self._dbus_available:
context.provide_app_dbus_interfaces = len(groups_set) == 0
context.dbus_base_bus_name = 'org.ros.qt_gui'
if context.provide_app_dbus_interfaces:
context.dbus_unique_bus_name = context.dbus_base_bus_name + '.pid%d' % os.getpid()
# provide pid of application via dbus
from .application_dbus_interface import ApplicationDBusInterface
_dbus_server = ApplicationDBusInterface(context.dbus_base_bus_name)
# determine host bus name, either based on pid given on command line or via dbus application interface if any other instance is available
if len(command_options_set) > 0 or len(embed_options_set) > 0:
host_pid = None
if self._options.command_pid is not None:
host_pid = self._options.command_pid
else:
try:
remote_object = SessionBus().get_object(context.dbus_base_bus_name, '/Application')
except DBusException:
pass
else:
remote_interface = Interface(remote_object, context.dbus_base_bus_name + '.Application')
host_pid = remote_interface.get_pid()
if host_pid is not None:
context.dbus_host_bus_name = context.dbus_base_bus_name + '.pid%d' % host_pid
# execute command on host application instance
if len(command_options_set) > 0:
if self._options.command_start_plugin is not None:
try:
remote_object = SessionBus().get_object(context.dbus_host_bus_name, '/PluginManager')
except DBusException:
(rc, msg) = (1, 'unable to communicate with GUI instance "%s"' % context.dbus_host_bus_name)
else:
remote_interface = Interface(remote_object, context.dbus_base_bus_name + '.PluginManager')
(rc, msg) = remote_interface.start_plugin(self._options.command_start_plugin, ' '.join(self._options.plugin_args))
if rc == 0:
print('qt_gui_main() started plugin "%s" in GUI "%s"' % (msg, context.dbus_host_bus_name))
else:
print('qt_gui_main() could not start plugin "%s" in GUI "%s": %s' % (self._options.command_start_plugin, context.dbus_host_bus_name, msg))
return rc
elif self._options.command_switch_perspective is not None:
remote_object = SessionBus().get_object(context.dbus_host_bus_name, '/PerspectiveManager')
remote_interface = Interface(remote_object, context.dbus_base_bus_name + '.PerspectiveManager')
remote_interface.switch_perspective(self._options.command_switch_perspective)
print('qt_gui_main() switched to perspective "%s" in GUI "%s"' % (self._options.command_switch_perspective, context.dbus_host_bus_name))
return 0
raise RuntimeError('Unknown command not handled')
# choose selected or default qt binding
setattr(sys, 'SELECT_QT_BINDING', self._options.qt_binding)
from python_qt_binding import QT_BINDING
from python_qt_binding.QtCore import qDebug, qInstallMsgHandler, QSettings, Qt, QtCriticalMsg, QtDebugMsg, QtFatalMsg, QTimer, QtWarningMsg
from python_qt_binding.QtGui import QAction, QIcon, QMenuBar
from .about_handler import AboutHandler
from .composite_plugin_provider import CompositePluginProvider
from .container_manager import ContainerManager
from .help_provider import HelpProvider
from .icon_loader import get_icon
from .main_window import MainWindow
from .minimized_dock_widgets_toolbar import MinimizedDockWidgetsToolbar
from .perspective_manager import PerspectiveManager
from .plugin_manager import PluginManager
def message_handler(type_, msg):
colored_output = 'TERM' in os.environ and 'ANSI_COLORS_DISABLED' not in os.environ
cyan_color = '\033[36m' if colored_output else ''
red_color = '\033[31m' if colored_output else ''
reset_color = '\033[0m' if colored_output else ''
if type_ == QtDebugMsg and self._options.verbose:
print(msg, file=sys.stderr)
elif type_ == QtWarningMsg:
print(cyan_color + msg + reset_color, file=sys.stderr)
elif type_ == QtCriticalMsg:
print(red_color + msg + reset_color, file=sys.stderr)
elif type_ == QtFatalMsg:
print(red_color + msg + reset_color, file=sys.stderr)
sys.exit(1)
qInstallMsgHandler(message_handler)
app = self.create_application(argv)
self._check_icon_theme_compliance()
settings = QSettings(QSettings.IniFormat, QSettings.UserScope, 'ros.org', self._settings_filename)
if len(embed_options_set) == 0:
if self._options.clear_config:
settings.clear()
main_window = MainWindow()
if self._options.on_top:
main_window.setWindowFlags(Qt.WindowStaysOnTopHint)
main_window.statusBar()
def sigint_handler(*args):
qDebug('\nsigint_handler()')
main_window.close()
signal.signal(signal.SIGINT, sigint_handler)
# the timer enables triggering the sigint_handler
timer = QTimer()
timer.start(500)
timer.timeout.connect(lambda: None)
# create own menu bar to share one menu bar on Mac
menu_bar = QMenuBar()
if 'darwin' in platform.platform().lower():
menu_bar.setNativeMenuBar(True)
else:
menu_bar.setNativeMenuBar(False)
if not self._options.lock_perspective:
main_window.setMenuBar(menu_bar)
file_menu = menu_bar.addMenu(menu_bar.tr('&File'))
action = QAction(file_menu.tr('&Quit'), file_menu)
action.setIcon(QIcon.fromTheme('application-exit'))
action.triggered.connect(main_window.close)
file_menu.addAction(action)
else:
app.setQuitOnLastWindowClosed(False)
main_window = None
menu_bar = None
self._add_plugin_providers()
# setup plugin manager
plugin_provider = CompositePluginProvider(self.plugin_providers)
plugin_manager = PluginManager(plugin_provider, settings, context, settings_prefix=plugin_manager_settings_prefix)
if self._options.list_plugins:
# output available plugins
print('\n'.join(sorted(plugin_manager.get_plugins().values())))
return 0
help_provider = HelpProvider()
plugin_manager.plugin_help_signal.connect(help_provider.plugin_help_request)
# setup perspective manager
if main_window is not None:
perspective_manager = PerspectiveManager(settings, context)
if self._options.list_perspectives:
# output available perspectives
print('\n'.join(sorted(perspective_manager.perspectives)))
return 0
else:
perspective_manager = None
if main_window is not None:
container_manager = ContainerManager(main_window, plugin_manager)
plugin_manager.set_main_window(main_window, menu_bar, container_manager)
if not self._options.freeze_layout:
minimized_dock_widgets_toolbar = MinimizedDockWidgetsToolbar(container_manager, main_window)
main_window.addToolBar(Qt.BottomToolBarArea, minimized_dock_widgets_toolbar)
plugin_manager.set_minimized_dock_widgets_toolbar(minimized_dock_widgets_toolbar)
if menu_bar is not None:
perspective_menu = menu_bar.addMenu(menu_bar.tr('P&erspectives'))
perspective_manager.set_menu(perspective_menu)
# connect various signals and slots
if perspective_manager is not None and main_window is not None:
# signal changed perspective to update window title
perspective_manager.perspective_changed_signal.connect(main_window.perspective_changed)
# signal new settings due to changed perspective
perspective_manager.save_settings_signal.connect(main_window.save_settings)
perspective_manager.restore_settings_signal.connect(main_window.restore_settings)
perspective_manager.restore_settings_without_plugin_changes_signal.connect(main_window.restore_settings)
if perspective_manager is not None and plugin_manager is not None:
perspective_manager.save_settings_signal.connect(plugin_manager.save_settings)
plugin_manager.save_settings_completed_signal.connect(perspective_manager.save_settings_completed)
perspective_manager.restore_settings_signal.connect(plugin_manager.restore_settings)
perspective_manager.restore_settings_without_plugin_changes_signal.connect(plugin_manager.restore_settings_without_plugins)
if plugin_manager is not None and main_window is not None:
# signal before changing plugins to save window state
plugin_manager.plugins_about_to_change_signal.connect(main_window.save_setup)
# signal changed plugins to restore window state
plugin_manager.plugins_changed_signal.connect(main_window.restore_state)
# signal save settings to store plugin setup on close
main_window.save_settings_before_close_signal.connect(plugin_manager.close_application)
# signal save and shutdown called for all plugins, trigger closing main window again
plugin_manager.close_application_signal.connect(main_window.close, type=Qt.QueuedConnection)
if main_window is not None and menu_bar is not None:
about_handler = AboutHandler(context.qtgui_path, main_window)
help_menu = menu_bar.addMenu(menu_bar.tr('&Help'))
action = QAction(file_menu.tr('&About'), help_menu)
action.setIcon(QIcon.fromTheme('help-about'))
action.triggered.connect(about_handler.show)
help_menu.addAction(action)
# set initial size - only used without saved configuration
if main_window is not None:
main_window.resize(600, 450)
main_window.move(100, 100)
# ensure that qt_gui/src is in sys.path
src_path = os.path.realpath(os.path.join(os.path.dirname(__file__), '..'))
if src_path not in sys.path:
sys.path.append(src_path)
# load specific plugin
plugin = None
plugin_serial = None
if self._options.embed_plugin is not None:
plugin = self._options.embed_plugin
plugin_serial = self._options.embed_plugin_serial
elif self._options.standalone_plugin is not None:
plugin = self._options.standalone_plugin
plugin_serial = 0
if plugin is not None:
plugins = plugin_manager.find_plugins_by_name(plugin)
if len(plugins) == 0:
print('qt_gui_main() found no plugin matching "%s"' % plugin)
return 1
elif len(plugins) > 1:
print('qt_gui_main() found multiple plugins matching "%s"\n%s' % (plugin, '\n'.join(plugins.values())))
return 1
plugin = plugins.keys()[0]
qDebug('QtBindingHelper using %s' % QT_BINDING)
plugin_manager.discover()
if self._options.reload_import:
qDebug('ReloadImporter() automatically reload all subsequent imports')
from .reload_importer import ReloadImporter
_reload_importer = ReloadImporter()
self._add_reload_paths(_reload_importer)
_reload_importer.enable()
# switch perspective
if perspective_manager is not None:
if plugin:
perspective_manager.set_perspective(plugin, hide_and_without_plugin_changes=True)
elif self._options.perspective_file:
perspective_manager.import_perspective_from_file(self._options.perspective_file, perspective_manager.HIDDEN_PREFIX + '__cli_perspective_from_file')
else:
perspective_manager.set_perspective(self._options.perspective)
# load specific plugin
if plugin:
plugin_manager.load_plugin(plugin, plugin_serial, self._options.plugin_args)
running = plugin_manager.is_plugin_running(plugin, plugin_serial)
if not running:
return 1
if self._options.standalone_plugin:
# use icon of standalone plugin (if available) for application
plugin_descriptor = plugin_manager.get_plugin_descriptor(plugin)
action_attributes = plugin_descriptor.action_attributes()
if 'icon' in action_attributes and action_attributes['icon'] is not None:
base_path = plugin_descriptor.attributes().get('plugin_path')
try:
icon = get_icon(action_attributes['icon'], action_attributes.get('icontype', None), base_path)
except UserWarning:
pass
else:
app.setWindowIcon(icon)
if main_window is not None:
main_window.show()
if sys.platform == 'darwin':
main_window.raise_()
return app.exec_()
def main(self,
argv=None,
standalone=None,
plugin_argument_provider=None,
plugin_manager_settings_prefix=''):
if argv is None:
argv = sys.argv
# extract --args and everything behind manually since argparse can not handle that
arguments = argv[1:]
# extract plugin specific args when not being invoked in standalone mode programmatically
if not standalone:
plugin_args = []
if '--args' in arguments:
index = arguments.index('--args')
plugin_args = arguments[index + 1:]
arguments = arguments[0:index + 1]
parser = ArgumentParser(os.path.basename(Main.main_filename),
add_help=False)
self.add_arguments(parser,
standalone=bool(standalone),
plugin_argument_provider=plugin_argument_provider)
self._options = parser.parse_args(arguments)
if standalone:
# rerun parsing to separate common arguments from plugin specific arguments
parser = ArgumentParser(os.path.basename(Main.main_filename),
add_help=False)
self.add_arguments(parser, standalone=bool(standalone))
self._options, plugin_args = parser.parse_known_args(arguments)
self._options.plugin_args = plugin_args
# set default values for options not available in standalone mode
if standalone:
self._options.freeze_layout = False
self._options.lock_perspective = False
self._options.multi_process = False
self._options.perspective = None
self._options.perspective_file = None
self._options.standalone_plugin = standalone
self._options.list_perspectives = False
self._options.list_plugins = False
self._options.command_pid = None
self._options.command_start_plugin = None
self._options.command_switch_perspective = None
self._options.embed_plugin = None
self._options.embed_plugin_serial = None
self._options.embed_plugin_address = None
# check option dependencies
try:
if self._options.plugin_args and not self._options.standalone_plugin and not self._options.command_start_plugin and not self._options.embed_plugin:
raise RuntimeError(
'Option --args can only be used together with either --standalone, --command-start-plugin or --embed-plugin option'
)
if self._options.freeze_layout and not self._options.lock_perspective:
raise RuntimeError(
'Option --freeze_layout can only be used together with the --lock_perspective option'
)
list_options = (self._options.list_perspectives,
self._options.list_plugins)
list_options_set = [
opt for opt in list_options if opt is not False
]
if len(list_options_set) > 1:
raise RuntimeError(
'Only one --list-* option can be used at a time')
command_options = (self._options.command_start_plugin,
self._options.command_switch_perspective)
command_options_set = [
opt for opt in command_options if opt is not None
]
if len(command_options_set) > 0 and not self._dbus_available:
raise RuntimeError(
'Without DBus support the --command-* options are not available'
)
if len(command_options_set) > 1:
raise RuntimeError(
'Only one --command-* option can be used at a time (except --command-pid which is optional)'
)
if len(command_options_set
) == 0 and self._options.command_pid is not None:
raise RuntimeError(
'Option --command_pid can only be used together with an other --command-* option'
)
embed_options = (self._options.embed_plugin,
self._options.embed_plugin_serial,
self._options.embed_plugin_address)
embed_options_set = [
opt for opt in embed_options if opt is not None
]
if len(command_options_set) > 0 and not self._dbus_available:
raise RuntimeError(
'Without DBus support the --embed-* options are not available'
)
if len(embed_options_set) > 0 and len(embed_options_set) < len(
embed_options):
raise RuntimeError(
'Missing option(s) - all \'--embed-*\' options must be set'
)
if len(embed_options_set) > 0 and self._options.clear_config:
raise RuntimeError(
'Option --clear-config can only be used without any --embed-* option'
)
groups = (list_options_set, command_options_set, embed_options_set)
groups_set = [opt for opt in groups if len(opt) > 0]
if len(groups_set) > 1:
raise RuntimeError(
'Options from different groups (--list, --command, --embed) can not be used together'
)
perspective_options = (self._options.perspective,
self._options.perspective_file)
perspective_options_set = [
opt for opt in perspective_options if opt is not None
]
if len(perspective_options_set) > 1:
raise RuntimeError(
'Only one --perspective-* option can be used at a time')
if self._options.perspective_file is not None and not os.path.isfile(
self._options.perspective_file):
raise RuntimeError(
'Option --perspective-file must reference existing file')
except RuntimeError as e:
print(str(e))
#parser.parse_args(['--help'])
# calling --help will exit
return 1
# set implicit option dependencies
if self._options.standalone_plugin is not None:
self._options.lock_perspective = True
# create application context containing various relevant information
from .application_context import ApplicationContext
context = ApplicationContext()
context.qtgui_path = self._qtgui_path
context.options = self._options
if self._dbus_available:
from dbus import DBusException, Interface, SessionBus
# non-special applications provide various dbus interfaces
if self._dbus_available:
context.provide_app_dbus_interfaces = len(groups_set) == 0
context.dbus_base_bus_name = 'org.ros.qt_gui'
if context.provide_app_dbus_interfaces:
context.dbus_unique_bus_name = context.dbus_base_bus_name + '.pid%d' % os.getpid(
)
# provide pid of application via dbus
from .application_dbus_interface import ApplicationDBusInterface
_dbus_server = ApplicationDBusInterface(
context.dbus_base_bus_name)
# determine host bus name, either based on pid given on command line or via dbus application interface if any other instance is available
if len(command_options_set) > 0 or len(embed_options_set) > 0:
host_pid = None
if self._options.command_pid is not None:
host_pid = self._options.command_pid
else:
try:
remote_object = SessionBus().get_object(
context.dbus_base_bus_name, '/Application')
except DBusException:
pass
else:
remote_interface = Interface(
remote_object,
context.dbus_base_bus_name + '.Application')
host_pid = remote_interface.get_pid()
if host_pid is not None:
context.dbus_host_bus_name = context.dbus_base_bus_name + '.pid%d' % host_pid
# execute command on host application instance
if len(command_options_set) > 0:
if self._options.command_start_plugin is not None:
try:
remote_object = SessionBus().get_object(
context.dbus_host_bus_name, '/PluginManager')
except DBusException:
(rc,
msg) = (1,
'unable to communicate with GUI instance "%s"' %
context.dbus_host_bus_name)
else:
remote_interface = Interface(
remote_object,
context.dbus_base_bus_name + '.PluginManager')
(rc, msg) = remote_interface.start_plugin(
self._options.command_start_plugin,
' '.join(self._options.plugin_args))
if rc == 0:
print('qt_gui_main() started plugin "%s" in GUI "%s"' %
(msg, context.dbus_host_bus_name))
else:
print(
'qt_gui_main() could not start plugin "%s" in GUI "%s": %s'
% (self._options.command_start_plugin,
context.dbus_host_bus_name, msg))
return rc
elif self._options.command_switch_perspective is not None:
remote_object = SessionBus().get_object(
context.dbus_host_bus_name, '/PerspectiveManager')
remote_interface = Interface(
remote_object,
context.dbus_base_bus_name + '.PerspectiveManager')
remote_interface.switch_perspective(
self._options.command_switch_perspective)
print(
'qt_gui_main() switched to perspective "%s" in GUI "%s"' %
(self._options.command_switch_perspective,
context.dbus_host_bus_name))
return 0
raise RuntimeError('Unknown command not handled')
# choose selected or default qt binding
setattr(sys, 'SELECT_QT_BINDING', self._options.qt_binding)
from python_qt_binding import QT_BINDING
from python_qt_binding.QtCore import qDebug, qInstallMessageHandler, QSettings, Qt, QtCriticalMsg, QtDebugMsg, QtFatalMsg, QTimer, QtWarningMsg
from python_qt_binding.QtGui import QIcon
from python_qt_binding.QtWidgets import QAction, QMenuBar
from .about_handler import AboutHandler
from .composite_plugin_provider import CompositePluginProvider
from .container_manager import ContainerManager
from .help_provider import HelpProvider
from .icon_loader import get_icon
from .main_window import MainWindow
from .minimized_dock_widgets_toolbar import MinimizedDockWidgetsToolbar
from .perspective_manager import PerspectiveManager
from .plugin_manager import PluginManager
# TODO PySide2 segfaults when invoking this custom message handler atm
if QT_BINDING != 'pyside':
def message_handler(type_, context, msg):
colored_output = 'TERM' in os.environ and 'ANSI_COLORS_DISABLED' not in os.environ
cyan_color = '\033[36m' if colored_output else ''
red_color = '\033[31m' if colored_output else ''
reset_color = '\033[0m' if colored_output else ''
if type_ == QtDebugMsg and self._options.verbose:
print(msg, file=sys.stderr)
elif type_ == QtWarningMsg:
print(cyan_color + msg + reset_color, file=sys.stderr)
elif type_ == QtCriticalMsg:
print(red_color + msg + reset_color, file=sys.stderr)
elif type_ == QtFatalMsg:
print(red_color + msg + reset_color, file=sys.stderr)
sys.exit(1)
qInstallMessageHandler(message_handler)
app = self.create_application(argv)
self._check_icon_theme_compliance()
settings = QSettings(QSettings.IniFormat, QSettings.UserScope,
'ros.org', self._settings_filename)
if len(embed_options_set) == 0:
if self._options.clear_config:
settings.clear()
main_window = MainWindow()
if self._options.on_top:
main_window.setWindowFlags(Qt.WindowStaysOnTopHint)
main_window.statusBar()
def sigint_handler(*args):
qDebug('\nsigint_handler()')
main_window.close()
signal.signal(signal.SIGINT, sigint_handler)
# the timer enables triggering the sigint_handler
timer = QTimer()
timer.start(500)
timer.timeout.connect(lambda: None)
menu_bar = main_window.menuBar()
file_menu = menu_bar.addMenu(menu_bar.tr('&File'))
action = QAction(file_menu.tr('&Quit'), file_menu)
action.setIcon(QIcon.fromTheme('application-exit'))
action.triggered.connect(main_window.close)
file_menu.addAction(action)
else:
app.setQuitOnLastWindowClosed(False)
main_window = None
menu_bar = None
self._add_plugin_providers()
# setup plugin manager
plugin_provider = CompositePluginProvider(self.plugin_providers)
plugin_manager = PluginManager(
plugin_provider,
settings,
context,
settings_prefix=plugin_manager_settings_prefix)
if self._options.list_plugins:
# output available plugins
print('\n'.join(sorted(plugin_manager.get_plugins().values())))
return 0
help_provider = HelpProvider()
plugin_manager.plugin_help_signal.connect(
help_provider.plugin_help_request)
# setup perspective manager
if main_window is not None:
perspective_manager = PerspectiveManager(settings, context)
if self._options.list_perspectives:
# output available perspectives
print('\n'.join(sorted(perspective_manager.perspectives)))
return 0
else:
perspective_manager = None
if main_window is not None:
container_manager = ContainerManager(main_window, plugin_manager)
plugin_manager.set_main_window(
main_window,
menu_bar if not self._options.lock_perspective else None,
container_manager)
if not self._options.freeze_layout:
minimized_dock_widgets_toolbar = MinimizedDockWidgetsToolbar(
container_manager, main_window)
main_window.addToolBar(Qt.BottomToolBarArea,
minimized_dock_widgets_toolbar)
plugin_manager.set_minimized_dock_widgets_toolbar(
minimized_dock_widgets_toolbar)
if menu_bar is not None and not self._options.lock_perspective:
perspective_menu = menu_bar.addMenu(menu_bar.tr('P&erspectives'))
perspective_manager.set_menu(perspective_menu)
# connect various signals and slots
if perspective_manager is not None and main_window is not None:
# signal changed perspective to update window title
perspective_manager.perspective_changed_signal.connect(
main_window.perspective_changed)
# signal new settings due to changed perspective
perspective_manager.save_settings_signal.connect(
main_window.save_settings)
perspective_manager.restore_settings_signal.connect(
main_window.restore_settings)
perspective_manager.restore_settings_without_plugin_changes_signal.connect(
main_window.restore_settings)
if perspective_manager is not None and plugin_manager is not None:
perspective_manager.save_settings_signal.connect(
plugin_manager.save_settings)
plugin_manager.save_settings_completed_signal.connect(
perspective_manager.save_settings_completed)
perspective_manager.restore_settings_signal.connect(
plugin_manager.restore_settings)
perspective_manager.restore_settings_without_plugin_changes_signal.connect(
plugin_manager.restore_settings_without_plugins)
if plugin_manager is not None and main_window is not None:
# signal before changing plugins to save window state
plugin_manager.plugins_about_to_change_signal.connect(
main_window.save_setup)
# signal changed plugins to restore window state
plugin_manager.plugins_changed_signal.connect(
main_window.restore_state)
# signal save settings to store plugin setup on close
main_window.save_settings_before_close_signal.connect(
plugin_manager.close_application)
# signal save and shutdown called for all plugins, trigger closing main window again
plugin_manager.close_application_signal.connect(
main_window.close, type=Qt.QueuedConnection)
if main_window is not None and menu_bar is not None:
about_handler = AboutHandler(context.qtgui_path, main_window)
help_menu = menu_bar.addMenu(menu_bar.tr('&Help'))
action = QAction(file_menu.tr('&About'), help_menu)
action.setIcon(QIcon.fromTheme('help-about'))
action.triggered.connect(about_handler.show)
help_menu.addAction(action)
# set initial size - only used without saved configuration
if main_window is not None:
main_window.resize(600, 450)
main_window.move(100, 100)
# ensure that qt_gui/src is in sys.path
src_path = os.path.realpath(
os.path.join(os.path.dirname(__file__), '..'))
if src_path not in sys.path:
sys.path.append(src_path)
# load specific plugin
plugin = None
plugin_serial = None
if self._options.embed_plugin is not None:
plugin = self._options.embed_plugin
plugin_serial = self._options.embed_plugin_serial
elif self._options.standalone_plugin is not None:
plugin = self._options.standalone_plugin
plugin_serial = 0
if plugin is not None:
plugins = plugin_manager.find_plugins_by_name(plugin)
if len(plugins) == 0:
print('qt_gui_main() found no plugin matching "%s"' % plugin)
return 1
elif len(plugins) > 1:
print(
'qt_gui_main() found multiple plugins matching "%s"\n%s' %
(plugin, '\n'.join(plugins.values())))
return 1
plugin = plugins.keys()[0]
qDebug('QtBindingHelper using %s' % QT_BINDING)
plugin_manager.discover()
if self._options.reload_import:
qDebug(
'ReloadImporter() automatically reload all subsequent imports')
from .reload_importer import ReloadImporter
_reload_importer = ReloadImporter()
self._add_reload_paths(_reload_importer)
_reload_importer.enable()
# switch perspective
if perspective_manager is not None:
if plugin:
perspective_manager.set_perspective(
plugin, hide_and_without_plugin_changes=True)
elif self._options.perspective_file:
perspective_manager.import_perspective_from_file(
self._options.perspective_file,
perspective_manager.HIDDEN_PREFIX +
os.path.basename(self._options.perspective_file))
else:
perspective_manager.set_perspective(self._options.perspective)
# load specific plugin
if plugin:
plugin_manager.load_plugin(plugin, plugin_serial,
self._options.plugin_args)
running = plugin_manager.is_plugin_running(plugin, plugin_serial)
if not running:
return 1
if self._options.standalone_plugin:
# use icon of standalone plugin (if available) for application
plugin_descriptor = plugin_manager.get_plugin_descriptor(
plugin)
action_attributes = plugin_descriptor.action_attributes()
if 'icon' in action_attributes and action_attributes[
'icon'] is not None:
base_path = plugin_descriptor.attributes().get(
'plugin_path')
try:
icon = get_icon(
action_attributes['icon'],
action_attributes.get('icontype', None), base_path)
except UserWarning:
pass
else:
app.setWindowIcon(icon)
if main_window is not None:
main_window.show()
if sys.platform == 'darwin':
main_window.raise_()
return app.exec_()
class Theta360ViewWidget(QWidget):
def __init__(self, plugin):
super(Theta360ViewWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('theta_viewer'), 'resource', 'Theta360ViewWidget.ui')
loadUi(ui_file, self)
self.plugin = plugin
self.position = (0.0, 0.0, 0.0)
self.orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
self.topicName = None
self.subscriber = None
# create GL view
self._glview = MyGLWidget()
self._glview.setAcceptDrops(True)
# backup and replace original paint method
# self.glView.paintGL is callbacked from QGLWidget
self._glview.paintGL_original = self._glview.paintGL
self._glview.paintGL = self.glview_paintGL
# backup and replace original mouse release method
self._glview.mouseReleaseEvent_original = self._glview.mouseReleaseEvent
self._glview.mouseReleaseEvent = self.glview_mouseReleaseEvent
# add GL view to widget layout
self.layout().addWidget(self._glview)
# init and start update timer with 40ms (25fps)
# updateTimeout is called with interval time
self.update_timer = QTimer(self)
self.update_timer.timeout.connect(self.update_timeout)
self.update_timer.start(40)
self.cnt = 0
## ==============================================
## rqt requires
def save_settings(self, plugin_settings, instance_settings):
view_matrix_string = repr(self._glview.get_view_matrix())
instance_settings.set_value('view_matrix', view_matrix_string)
def restore_settings(self, plugin_settings, instance_settings):
view_matrix_string = instance_settings.value('view_matrix')
try:
view_matrix = eval(view_matrix_string)
except Exception:
view_matrix = None
if view_matrix is not None:
self._glview.set_view_matrix(view_matrix)
else:
self.set_default_view()
def shutdown_plugin(self):
self.unregister_topic()
## ==============================================
## QGLWidget requires
def set_default_view(self):
self._glview.makeCurrent()
self._glview.reset_view()
self._glview.rotate((0, 0, 1), 45)
self._glview.rotate((1, 0, 0), -45)
self._glview.translate((0, 0, -200))
def update_timeout(self):
print self.cnt
self._glview.makeCurrent()
self._glview.updateGL()
glRotated(45 + self.cnt, 0, 0, 1)
def glview_paintGL(self):
self._glview.paintGL_original()
self.draw_basic_objects()
if self.cnt == 0:
print 'DRAW!'
self.qimage = QImage('/home/matsunolab/Pictures/testimage_big.jpg', 'JPEG') # GL_TEXTURE_2D
self.texture = self._glview.get_texture(self.qimage)
my_glfuncs.map_texture_on_sphere2(self.texture, 1500, 30, 30)
self.cnt += 1
def glview_mouseReleaseEvent(self, event):
if event.button() == Qt.RightButton:
menu = QMenu(self._glview)
action = QAction(self._glview.tr("Reset view"), self._glview)
menu.addAction(action)
action.triggered.connect(self.set_default_view)
menu.exec_(self._glview.mapToGlobal(event.pos()))
## ==============================================
## ROS requires
def message_callback(self, message):
self.position = (message.position.x, message.position.y, message.position.z)
self.orientation = (message.orientation.x, message.orientation.y, message.orientation.z, message.orientation.w)
def unregister_topic(self):
if self.subscriber:
self.subscriber.unregister()
def subscribe_topic(self, topicName):
msgClass, self.topicName, _ = get_topic_class(topicName)
self.subscriber = rospy.Subscriber(self.topicName, msgClass, self.message_callback)
## ==============================================
## QT requires(?)
@Slot('QDragEnterEvent*')
def dragEnterEvent(self, event):
if not event.mimeData().hasText():
if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
qWarning(
'Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
return
item = event.source().selectedItems()[0]
ros_topic_name = item.data(0, Qt.UserRole)
if ros_topic_name is None:
qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topicName_)')
return
# TODO: do some checks for valid topic here
event.acceptProposedAction()
@Slot('QDropEvent*')
def dropEvent(self, event):
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
self.unregister_topic()
self.subscribe_topic(topic_name)
## ==============================================
## For drawing
def draw_basic_objects(self):
glLineWidth(5)
my_glfuncs.draw_axis()
glLineWidth(1)
glColor4f(1.0, 1.0, 1.0, 1.0)
my_glfuncs.draw_grand_gradation(200, 200, 10, 2)
class StatusLightWidget(QWidget):
_UNKNOWN_COLOR = QColor("#dddddd")
_SUCCESS_COLOR = QColor("#18FFFF")
_WARN_COLOR = QColor("#FFCA00")
_ERROR_COLOR = QColor("#F44336")
def __init__(self):
super(StatusLightWidget, self).__init__()
self.lock = Lock()
self.status_sub = None
self.status = 0
self._status_topics = []
self._update_topic_timer = QTimer(self)
self._update_topic_timer.timeout.connect(self.updateTopics)
self._update_topic_timer.start(1000)
self._active_topic = None
self._dialog = ComboBoxDialog()
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.redraw)
self._update_plot_timer.start(1000 / 15)
def redraw(self):
self.update()
def paintEvent(self, event):
with self.lock:
if self.status == 1:
color = self._SUCCESS_COLOR
elif self.status == 2:
color = self._WARN_COLOR
else:
color = self._UNKNOWN_COLOR
rect = event.rect()
qp = QPainter()
qp.begin(self)
radius = min(rect.width(), rect.height()) - 100
qp.setFont(QFont('Helvetica', 100))
qp.setPen(QPen(QBrush(color), 50))
qp.setBrush(color)
qp.drawEllipse((rect.width() - radius) / 2,
(rect.height() - radius) / 2, radius, radius)
qp.end()
return
def trigger_configuration(self):
self._dialog.exec_()
self.setupSubscriber(self._status_topics[self._dialog.number])
def updateTopics(self):
need_to_update = False
for (topic, topic_type) in rospy.get_published_topics():
if topic_type == "std_msgs/UInt8":
if not topic in self._status_topics:
self._status_topics.append(topic)
need_to_update = True
if need_to_update:
self._status_topics = sorted(self._status_topics)
self._dialog.combo_box.clear()
for topic in self._status_topics:
self._dialog.combo_box.addItem(topic)
if self._active_topic:
if self._active_topic not in self._status_topics:
self._status_topics.append(self._active_topic)
self._dialog.combo_box.addItem(self._active_topic)
self._dialog.combo_box.setCurrentIndex(
self._status_topics.index(self._active_topic))
def setupSubscriber(self, topic):
if self.status_sub:
self.status_sub.unregister()
self.status_sub = rospy.Subscriber(topic, UInt8, self.statusCallback)
self._active_topic = topic
def onActivated(self, number):
self.setupSubscriber(self._status_topics[number])
def statusCallback(self, msg):
self.status = msg.data
def save_settings(self, plugin_settings, instance_settings):
if self._active_topic:
instance_settings.set_value("active_topic", self._active_topic)
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.value("active_topic"):
topic = instance_settings.value("active_topic")
self._dialog.combo_box.addItem(topic)
self.setupSubscriber(topic)
class MonitorDashWidget(IconToolButton):
"""
A widget which brings up the rqt_robot_monitor.
Times out after certain period of time (set as 5 sec as of Apr 2013)
without receiving diagnostics msg ('/diagnostics_toplevel_state' of
DiagnosticStatus type), status becomes as 'stale'.
:param context: The plugin context to create the monitor in.
:type context: qt_gui.plugin_context.PluginContext
"""
def __init__(self, context, icon_paths=[]):
self._graveyard = []
ok_icon = ['bg-green.svg', 'ic-diagnostics.svg']
warn_icon = ['bg-yellow.svg', 'ic-diagnostics.svg',
'ol-warn-badge.svg']
err_icon = ['bg-red.svg', 'ic-diagnostics.svg', 'ol-err-badge.svg']
stale_icon = ['bg-grey.svg', 'ic-diagnostics.svg',
'ol-stale-badge.svg']
icons = [ok_icon, warn_icon, err_icon, stale_icon]
super(MonitorDashWidget, self).__init__('MonitorWidget', icons,
icon_paths=icon_paths)
self.setFixedSize(self._icons[0].actualSize(QSize(50, 30)))
self._monitor = None
self._close_mutex = QMutex()
self._show_mutex = QMutex()
self._last_update = rospy.Time.now()
self.context = context
self.clicked.connect(self._show_monitor)
self._monitor_shown = False
self.setToolTip('Diagnostics')
self._diagnostics_toplevel_state_sub = rospy.Subscriber(
'diagnostics_toplevel_state',
DiagnosticStatus, self.toplevel_state_callback)
self._top_level_state = -1
self._stall_timer = QTimer()
self._stall_timer.timeout.connect(self._stalled)
self._stalled()
self._plugin_settings = None
self._instance_settings = None
def toplevel_state_callback(self, msg):
self._is_stale = False
self._stall_timer.start(5000)
if self._top_level_state != msg.level:
if (msg.level >= 2):
self.update_state(2)
self.setToolTip("Diagnostics: Error")
elif (msg.level == 1):
self.update_state(1)
self.setToolTip("Diagnostics: Warning")
else:
self.update_state(0)
self.setToolTip("Diagnostics: OK")
self._top_level_state = msg.level
def _stalled(self):
self._stall_timer.stop()
self._is_stale = True
self.update_state(3)
self._top_level_state = 3
self.setToolTip("Diagnostics: Stale\nNo message received on " +
"dashboard_agg in the last 5 seconds")
def _show_monitor(self):
with QMutexLocker(self._show_mutex):
try:
if self._monitor_shown:
self.context.remove_widget(self._monitor)
self._monitor_close()
self._monitor_shown = False
else:
self._monitor = RobotMonitorWidget(self.context,
'/diagnostics_agg')
if self._plugin_settings:
self._monitor.restore_settings(self._plugin_settings,
self._instance_settings)
self.context.add_widget(self._monitor)
self._monitor_shown = True
except Exception:
if self._monitor_shown == False:
raise
#TODO: when closeEvents is available fix this hack
# (It ensures the button will toggle correctly)
self._monitor_shown = False
self._show_monitor()
def _monitor_close(self):
if self._monitor_shown:
with QMutexLocker(self._close_mutex):
if self._plugin_settings:
self._monitor.save_settings(self._plugin_settings,
self._instance_settings)
self._monitor.shutdown()
self._monitor.close()
self._graveyard.append(self._monitor)
self._monitor = None
def shutdown_widget(self):
self._stall_timer.stop()
if self._monitor:
self._monitor.shutdown()
self._diagnostics_toplevel_state_sub.unregister()
def save_settings(self, plugin_settings, instance_settings):
if self._monitor_shown:
self._monitor.save_settings(self._plugin_settings,
self._instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._plugin_settings = plugin_settings
self._instance_settings = instance_settings
class RuntimeMonitorWidget(QWidget):
def __init__(self, topic="/diagnostics"):
super(RuntimeMonitorWidget, self).__init__()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'runtime_monitor_widget.ui')
loadUi(ui_file, self)
self.setObjectName('RuntimeMonitorWidget')
self._mutex = threading.Lock()
self._error_icon = QIcon.fromTheme('dialog-error')
self._warning_icon = QIcon.fromTheme('dialog-warning')
self._ok_icon = QIcon.fromTheme('dialog-information')
self._stale_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Stale (0)'])
self._stale_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._stale_node)
self._error_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Errors (0)'])
self._error_node.setIcon(0, self._error_icon)
self.tree_widget.addTopLevelItem(self._error_node)
self._warning_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Warnings (0)'])
self._warning_node.setIcon(0, self._warning_icon)
self.tree_widget.addTopLevelItem(self._warning_node)
self._ok_node = QTreeWidgetItem(self.tree_widget.invisibleRootItem(), ['Ok (0)'])
self._ok_node.setIcon(0, self._ok_icon)
self.tree_widget.addTopLevelItem(self._ok_node)
self.tree_widget.itemSelectionChanged.connect(self._on_item_selected)
self.keyPressEvent = self._on_key_press
self._name_to_item = {}
self._new_errors_callback = None
self._subscriber = rospy.Subscriber(topic, DiagnosticArray, self._diagnostics_callback)
self._timer = QTimer()
self._timer.timeout.connect(self._on_timer)
self._timer.start(5000)
self._messages = []
self._used_items = 0
def __del__(self):
if self._subscriber is not None:
self._subscriber.unregister()
self._subscriber = None
def shutdown(self):
"""
Unregisters diagnostics subscriber for clean shutdown
"""
if rospy.is_shutdown():
return
if self._subscriber is not None:
self._subscriber.unregister()
self._subscriber = None
def change_diagnostic_topic(self, topic):
"""
Changes diagnostics topic name. Must be of type diagnostic_msgs/DiagnosticArray
"""
if len(topic) == 0:
self.reset_monitor()
return
if self._subscriber is not None:
self._subscriber.unregister()
self._subscriber = rospy.Subscriber(str(topic), DiagnosticArray, self._diagnostics_callback)
self.reset_monitor()
def reset_monitor(self):
"""
Removes all values from monitor display, resets buffers
"""
self._name_to_item = {} # Reset all stale topics
self._messages = []
self._clear_tree()
def _clear_tree(self):
for index in range(self._stale_node.childCount()):
self._stale_node.removeChild(self._stale_node.child(index))
for index in range(self._error_node.childCount()):
self._error_node.removeChild(self._error_node.child(index))
for index in range(self._warning_node.childCount()):
self._warning_node.removeChild(self._warning_node.child(index))
for index in range(self._ok_node.childCount()):
self._ok_node.removeChild(self._ok_node.child(index))
self._update_root_labels()
def _diagnostics_callback(self, message):
with self._mutex:
self._messages.append(message)
self.new_message(rospy.get_rostime())
def new_message(self, stamp=None):
with self._mutex:
had_errors = False
for message in self._messages:
for status in message.status:
was_selected = False
if (self._name_to_item.has_key(status.name)):
item = self._name_to_item[status.name]
if self.tree_widget.selectedItems() != [] and self.tree_widget.selectedItems()[0] == item:
was_selected = True
if (item.status.level == 2 and status.level != 2):
had_errors = True
self._update_item(item, status, was_selected, stamp)
else:
self._create_item(status, was_selected, True, stamp)
if (status.level == 2):
had_errors = True
self._messages = []
if (had_errors and self._new_errors_callback != None):
self._new_errors_callback()
self._update_root_labels()
self.update()
def _update_item(self, item, status, was_selected, stamp):
change_parent = False
if (item.status.level != status.level):
change_parent = True
if (change_parent):
if (item.status.level == 0):
self._ok_node.removeChild(item.tree_node)
elif (item.status.level == 1):
self._warning_node.removeChild(item.tree_node)
elif (item.status.level == -1):
self._stale_node.removeChild(item.tree_node)
else:
self._error_node.removeChild(item.tree_node)
if (status.level == 0):
parent_node = self._ok_node
elif (status.level == 1):
parent_node = self._warning_node
elif (status.level == -1):
parent_node = self._stale_node
else:
parent_node = self._error_node
item.tree_node.setText(0, status.name + ": " + status.message)
item.stamp = stamp
item.tree_node.setData(0, Qt.UserRole, item)
parent_node.addChild(item.tree_node)
if (status.level > 1 or status.level == -1):
parent_node.setExpanded(True)
parent_node.sortChildren(0, Qt.AscendingOrder)
if (was_selected):
item.tree_node.setSelected(True)
else:
item.tree_node.setText(0, status.name + ": " + status.message)
item.status = status
if (was_selected):
self._fillout_info(item.tree_node)
item.mark = True
def _create_item(self, status, select, expand_if_error, stamp):
if (status.level == 0):
parent_node = self._ok_node
elif (status.level == 1):
parent_node = self._warning_node
elif (status.level == -1):
parent_node = self._stale_node
else:
parent_node = self._error_node
item = TreeItem(status, QTreeWidgetItem(parent_node, [status.name + ": " + status.message]), stamp)
item.tree_node.setData(0, Qt.UserRole, item)
parent_node.addChild(item.tree_node)
self._name_to_item[status.name] = item
parent_node.sortChildren(0, Qt.AscendingOrder)
if (select):
item.tree_node.setSelected(True)
if (expand_if_error and (status.level > 1 or status.level == -1)):
parent_node.setExpanded(True)
item.mark = True
return item
def _fillout_info(self, node):
item = node.data(0, Qt.UserRole)
if (item == None):
return
status = item.status
s = cStringIO.StringIO()
s.write("<html><body>")
s.write("<b>Component</b>: %s<br>\n" % (status.name))
s.write("<b>Message</b>: %s<br>\n" % (status.message))
s.write("<b>Hardware ID</b>: %s<br><br>\n\n" % (status.hardware_id))
s.write('<table border="1" cellpadding="2" cellspacing="0">')
for value in status.values:
value.value = value.value.replace("\n", "<br>")
s.write("<tr><td><b>%s</b></td> <td>%s</td></tr>\n" % (value.key, value.value))
s.write("</table></body></html>")
self.html_browser.setHtml(s.getvalue())
def _on_item_selected(self):
current_item = self.tree_widget.selectedItems()
if current_item is not None:
self._fillout_info(current_item[0])
def _on_key_press(self, event):
key = event.key()
if key == Qt.Key_Delete:
nodes = self.tree_widget.selectedItems()
if (nodes != [] and nodes[0] not in (self._ok_node, self._warning_node, self._stale_node, self._error_node)):
item = nodes[0].data(0, Qt.UserRole)
if (item.status.level == 0):
self._ok_node.removeChild(item.tree_node)
elif (item.status.level == 1):
self._warning_node.removeChild(item.tree_node)
elif (item.status.level == -1):
self._stale_node.removeChild(item.tree_node)
else:
self._error_node.removeChild(item.tree_node)
del self._name_to_item[item.status.name]
else:
event.Skip()
self._update_root_labels()
self.update()
def _on_timer(self):
for name, item in self._name_to_item.iteritems():
node = item.tree_node
if (item != None):
if (not item.mark):
was_selected = False
selected = self.tree_widget.selectedItems()
if selected != [] and selected[0] == node:
was_selected = True
new_status = copy.deepcopy(item.status)
new_status.level = -1
self._update_item(item, new_status, was_selected, item.stamp)
item.mark = False
self._update_root_labels()
self.update()
def set_new_errors_callback(self, callback):
self._new_errors_callback = callback
def get_num_errors(self):
return self._error_node.childCount() + self._stale_node.childCount()
def get_num_warnings(self):
return self._warning_node.childCount()
def get_num_ok(self):
return self._ok_node.childCount()
def _update_root_labels(self):
self._stale_node.setText(0, "Stale (%s)" % (self._stale_node.childCount()))
self._error_node.setText(0, "Errors (%s)" % (self._error_node.childCount()))
self._warning_node.setText(0, "Warnings (%s)" % (self._warning_node.childCount()))
self._ok_node.setText(0, "Ok (%s)" % (self._ok_node.childCount()))
