def init_sliders(self):
sliderbox = self._widget.findChild(QLayout, 'Sliders')
graph_button = QPushButton()
graph_button.setCheckable(True)
graph_button.setText("Graph Off")
graph_button.toggle()
graph_button.clicked.connect(self.set_graph_state)
self.graph_button = graph_button
firstCol = QVBoxLayout()
firstCol.addWidget(graph_button)
sliderbox.addLayout(firstCol)
self.sliders = []
all_rows_layout = QVBoxLayout()
chan_idx = 0
for num_channels_row in self.settings['num_channels']:
row_layout = QHBoxLayout()
for i in range(num_channels_row):
idx = chan_idx * 1
slider_group = {
'slider_p': None,
'number_p': None,
'slider_v': None,
'number_v': None,
'on_off': None
}
layout_cluster = QVBoxLayout()
slider_cluster = QHBoxLayout()
label = QLabel()
label.setText("Chan. %d" % (idx + 1))
label.setAlignment(Qt.AlignCenter)
layout_cluster.addWidget(label)
for j in range(2):
layout = QVBoxLayout()
layout.setAlignment(Qt.AlignHCenter)
slider = QSlider(Qt.Vertical)
slider.setMinimum(0)
slider.setMaximum(255)
slider.setValue(
self.settings['valve_offsets'][chan_idx][j])
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(5)
spinbox = QSpinBox()
spinbox.setRange(0, 255)
spinbox.setValue(
self.settings['valve_offsets'][chan_idx][j])
slider.valueChanged.connect(spinbox.setValue)
spinbox.valueChanged.connect(slider.setValue)
cb_function_curr = lambda value, idx=idx: self.send_slider_value(
idx, value)
slider.valueChanged.connect(cb_function_curr)
label = QLabel()
label.setAlignment(Qt.AlignCenter)
if j == 0:
slider_group['slider_p'] = slider
slider_group['number_p'] = spinbox
label.setText("P")
else:
slider_group['slider_v'] = slider
slider_group['number_v'] = spinbox
label.setText("V")
labelmax = QLabel()
labelmax.setAlignment(Qt.AlignCenter)
labelmax.setText("255")
labelmin = QLabel()
labelmin.setAlignment(Qt.AlignCenter)
labelmin.setText("0")
layout.addWidget(label)
layout.addWidget(labelmax)
layout.addWidget(slider, Qt.AlignHCenter)
layout.addWidget(labelmin)
layout.addWidget(spinbox, Qt.AlignHCenter)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
slider_cluster.addLayout(layout)
on_button = QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][chan_idx]:
on_button.toggle()
on_button.setText("On")
on_button.clicked.connect(
lambda state, idx=idx: self.send_channel_state(idx, state))
slider_group['on_off'] = on_button
layout_cluster.addLayout(slider_cluster)
layout_cluster.addWidget(on_button)
row_layout.addLayout(layout_cluster)
row_layout.addSpacing(20)
self.sliders.append(slider_group)
chan_idx += 1
all_rows_layout.addLayout(row_layout)
sliderbox.addLayout(all_rows_layout)
class PathExecution(Plugin):
StepByStepParam = "/agimus/step_by_step"
StepTopic = "/agimus/step"
PathExecutionTopic = "/agimus/start_path"
PublishStateService = "/agimus/sot/publish_state"
EventDoneTopic = "/agimus/sot/event/done"
def __init__(self, context):
super(PathExecution, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName("PathExecution")
while not rospy.has_param(PathExecution.StepByStepParam):
from time import sleep
rospy.loginfo("Waiting for parameter " + PathExecution.StepByStepParam)
sleep(0.1)
self.step_publisher = rospy.Publisher(
PathExecution.StepTopic, EmptyMsg, queue_size=1
)
self.path_execution_publisher = rospy.Publisher(
PathExecution.PathExecutionTopic, UInt32, queue_size=1
)
self.event_done_publisher = rospy.Publisher(
PathExecution.EventDoneTopic, Int32, queue_size=1
)
# Create QWidget
self._widget = QWidget()
self._layout = QGridLayout(self._widget)
self._layout.setColumnStretch(0, 1)
self._layout.setColumnStretch(3, 1)
def add_space(row):
spacer = QFrame()
spacer.setFrameShape(QFrame.HLine)
# self._layout.addWidget (spacer, row, 0, 1, 4)
self._layout.addWidget(spacer, row, 1, 1, 2)
row = 0
# Step by step
self._layout.addWidget(QLabel("Step by step"), row, 1, 1, 2, Qt.AlignCenter)
row += 1
step_by_step_spin_box = QSpinBox()
step_by_step_spin_box.setRange(0, 4)
step_by_step_spin_box.setToolTip(
"""Set the step by step level, from 0 (non-stop) to 4 (stop very often)."""
)
step_by_step_spin_box.setValue(rospy.get_param(PathExecution.StepByStepParam))
# step_by_step_spin_box.connect (self.setStepByStepParam)
step_by_step_spin_box.valueChanged.connect(
lambda val: rospy.set_param(PathExecution.StepByStepParam, val)
)
self._layout.addWidget(QLabel("Level: "), row, 1, Qt.AlignRight)
self._layout.addWidget(step_by_step_spin_box, row, 2)
row += 1
self._one_step_button = QPushButton("Execute one step")
self._one_step_button.clicked.connect(lambda x: self.step_publisher.publish())
self._layout.addWidget(self._one_step_button, row, 2)
row += 1
# Spacer
add_space(row)
row += 1
## Path execution
self._layout.addWidget(QLabel("Path execution"), row, 1, 1, 2, Qt.AlignCenter)
row += 1
self.path_index_spin_box = QSpinBox()
self.path_index_spin_box.setRange(0, 10000)
execute_path = QPushButton("Execute path")
execute_path.clicked.connect(
lambda unused: self.path_execution_publisher.publish(
self.path_index_spin_box.value()
)
)
self._layout.addWidget(self.path_index_spin_box, row, 1)
self._layout.addWidget(execute_path, row, 2)
row += 1
# Spacer
add_space(row)
row += 1
## Tools
self._layout.addWidget(QLabel("Tools"), row, 1, 1, 2, Qt.AlignCenter)
row += 1
publish_state = QPushButton("Request SoT to publish its state")
publish_state.clicked.connect(lambda u: self.publishState())
self._layout.addWidget(publish_state, row, 2)
row += 1
send_event_done = QPushButton("Send event done.")
send_event_done.clicked.connect(lambda x: self.event_done_publisher.publish(0))
self._layout.addWidget(send_event_done, row, 2)
row += 1
geom_simu_paused = QPushButton("Pause geometric simu.")
geom_simu_paused.setCheckable(True)
geom_simu_paused.setChecked(rospy.get_param("/geometric_simu/paused", False))
geom_simu_paused.clicked.connect(
lambda checked: rospy.set_param("/geometric_simu/paused", checked)
)
self._layout.addWidget(geom_simu_paused, row, 2)
row += 1
space = QWidget()
space.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
self._layout.addWidget(space, row, 1, 1, 2)
row += 1
# self._layout.addWidget (None, row, 0, 1, 4)
# Give QObjects reasonable names
self._widget.setObjectName("SupervisionUi")
# 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)
def publishState(self):
try:
rospy.wait_for_service(PathExecution.PublishStateService, 0.5)
publish_state = rospy.ServiceProxy(
PathExecution.PublishStateService, EmptySrv
)
publish_state()
except rospy.exceptions.ROSException as e:
QMessageBox.warning(self, "Service unreachable.", str(e))
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 __init__(self, context):
super(Supervision, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Supervision')
while not rospy.has_param(Supervision.StepByStepParam):
rospy.sleep(0.1)
self.step_publisher = rospy.Publisher(Supervision.StepTopic,
EmptyMsg,
queue_size=1)
self.path_execution_publisher = rospy.Publisher(
Supervision.PathExecutionTopic, UInt32, queue_size=1)
# Create QWidget
self._widget = QWidget()
self._layout = QGridLayout(self._widget)
self._layout.setColumnStretch(0, 1)
self._layout.setColumnStretch(3, 1)
def add_space(row):
spacer = QFrame()
spacer.setFrameShape(QFrame.HLine)
#self._layout.addWidget (spacer, row, 0, 1, 4)
self._layout.addWidget(spacer, row, 1, 1, 2)
row = 0
# Step by step
self._layout.addWidget(QLabel("Step by step"), row, 1, 1, 2,
Qt.AlignCenter)
row += 1
step_by_step_spin_box = QSpinBox()
step_by_step_spin_box.setRange(0, 4)
step_by_step_spin_box.setToolTip(
"""Set the step by step level, from 0 (non-stop) to 4 (stop very often)."""
)
step_by_step_spin_box.setValue(
rospy.get_param(Supervision.StepByStepParam))
# step_by_step_spin_box.connect (self.setStepByStepParam)
step_by_step_spin_box.valueChanged.connect(
lambda val: rospy.set_param(Supervision.StepByStepParam, val))
self._layout.addWidget(QLabel("Level: "), row, 1, Qt.AlignRight)
self._layout.addWidget(step_by_step_spin_box, row, 2)
row += 1
self._one_step_button = QPushButton("Execute one step")
self._one_step_button.clicked.connect(
lambda x: self.step_publisher.publish())
self._layout.addWidget(self._one_step_button, row, 2)
row += 1
# Spacer
add_space(row)
row += 1
## Path execution
self._layout.addWidget(QLabel("Path execution"), row, 1, 1, 2,
Qt.AlignCenter)
row += 1
self.path_index_spin_box = QSpinBox()
# step_by_step_spin_box.setRange(0,100000)
execute_path = QPushButton("Execute path")
execute_path.clicked.connect(
lambda unused: self.path_execution_publisher.publish(
self.path_index_spin_box.value()))
self._layout.addWidget(self.path_index_spin_box, row, 1)
self._layout.addWidget(execute_path, row, 2)
row += 1
# Spacer
add_space(row)
row += 1
## Tools
self._layout.addWidget(QLabel("Tools"), row, 1, 1, 2, Qt.AlignCenter)
row += 1
publish_state = QPushButton("Request SoT to publish its state")
publish_state.clicked.connect(lambda u: self.publishState())
self._layout.addWidget(publish_state, row, 2)
row += 1
#self._layout.addWidget (None, row, 0, 1, 4)
# Give QObjects reasonable names
self._widget.setObjectName('SupervisionUi')
# 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)
class SpectrogramPlugin(Plugin):
update_signal = QtCore.pyqtSignal()
subscriber_signal = QtCore.pyqtSignal(str)
module_num_signal = QtCore.pyqtSignal()
value_signal = QtCore.pyqtSignal(int)
vib_freq = 48000
overlap = 50
stft_freq = 200
frame_time = 1
frame_length = frame_time * stft_freq
label_flag = 0
lowcut_freq = 0
highcut_freq = vib_freq / 2
v_max = 0
v_min = 0
colorbarflag = 0
cnt_fig = 0
def __init__(self, context):
super(SpectrogramPlugin, self).__init__(context)
self.setObjectName('Spectrogram')
sns.set(style="whitegrid", palette="bright", color_codes=True)
self._widget = QWidget()
layout = QVBoxLayout()
self._widget.setLayout(layout)
layout_ = QHBoxLayout()
self.lbl_topic = QLabel('Topic:')
layout_.addWidget(self.lbl_topic)
self.le_topic = QLineEdit()
layout_.addWidget(self.le_topic)
self.apply_topic = QPushButton("Apply")
self.apply_topic.clicked.connect(self.apply_clicked_topic)
layout_.addWidget(self.apply_topic)
layout.addLayout(layout_)
layout_ = QHBoxLayout()
self.lbl_lcf = QLabel('Low-cut Freq.[Hz]:')
layout_.addWidget(self.lbl_lcf)
self.spb_lcf = QSpinBox()
self.spb_lcf.setRange(0, 50)
self.spb_lcf.setValue(0)
layout_.addWidget(self.spb_lcf)
self.apply_lcf = QPushButton("Apply")
self.apply_lcf.clicked.connect(self.apply_clicked_lcf)
layout_.addWidget(self.apply_lcf)
layout.addLayout(layout_)
layout_ = QHBoxLayout()
self.lbl_hcf = QLabel('High-cut Freq.[Hz]:')
layout_.addWidget(self.lbl_hcf)
self.spb_hcf = QSpinBox()
self.spb_hcf.setRange(50, self.vib_freq / 2)
self.spb_hcf.setValue(self.vib_freq / 2)
layout_.addWidget(self.spb_hcf)
self.apply_hcf = QPushButton("Apply")
self.apply_hcf.clicked.connect(self.apply_clicked_hcf)
layout_.addWidget(self.apply_hcf)
layout.addLayout(layout_)
#self.fig, self.axes = plt.subplots(2, 1, sharex=True)
self.fig = plt.figure()
self.ax = self.fig.add_subplot(1, 1, 1)
self.canvas = FigureCanvas(self.fig)
self.fig.tight_layout()
layout.addWidget(self.canvas)
context.add_widget(self._widget)
self.update_signal.connect(self.update_spectrogram)
self.subscriber_signal.connect(self.update_subscriber)
self.subscriber_signal.emit('spectrum')
def changed_spinbox_value(self, n):
self.valueChanged.emit(n)
def spectrum_callback(self, data):
nch = data.nch
len = data.nfreq
if self.spectrogram is None:
self.spectrogram = zeros([len, self.frame_length, nch])
#self.spectrogram = ones([len, self.frame_length, 4*nch])*log(1e-8)
self.spectrogram = roll(self.spectrogram, -1, 1)
for i in range(nch):
s = array(data.data).reshape([nch, len, 2])[i]
s = linalg.norm(s, axis=1)
s += 1e-8
log(s, s)
self.spectrogram[:, -1, i] = s
#if data.header.seq % 2 == 0:
if self.v_max < self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].max():
self.v_max = self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].max()
if self.v_min > self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].min():
self.v_min = self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].min()
self.update_signal.emit()
def apply_clicked_topic(self):
self.update_subscriber(self.le_topic.displayText())
def apply_clicked_lcf(self):
self.lowcut_freq = self.spb_lcf.value()
def apply_clicked_hcf(self):
self.highcut_freq = self.spb_hcf.value()
def update_spectrogram(self):
if self.spectrogram is not None:
yticks = [
0, self.highcut_freq / 2 / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq,
self.highcut_freq / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq,
self.highcut_freq / 2 * 3 / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq,
self.highcut_freq * 2 / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq
]
yticks_label = [
self.lowcut_freq, self.highcut_freq / 4, self.highcut_freq / 2,
self.highcut_freq / 4 * 3, self.highcut_freq
]
xticks = [
0, self.frame_length / 4 - 1, self.frame_length / 2 - 1,
self.frame_length * 3 / 4 - 1, self.frame_length - 1
]
xticks_label = [
-self.frame_time * 2, -self.frame_time / 2 * 3,
-self.frame_time, -self.frame_time / 2, 0
]
font_size = 13
if self.cnt_fig % 40 == 0:
self.ax.clear()
#self.im = self.ax.imshow(self.spectrogram[int(self.lowcut_freq*100/self.overlap/self.stft_freq):int(self.highcut_freq*100/self.overlap/self.stft_freq)+1,:,0], aspect="auto", origin="lower", cmap="winter", interpolation='none', vmin=self.v_min, vmax=self.v_max)
self.im = self.ax.imshow(self.spectrogram[
int(self.lowcut_freq * 100 / self.overlap /
self.stft_freq):int(self.highcut_freq * 100 /
self.overlap / self.stft_freq) +
1, :, 0],
aspect="auto",
origin="lower",
cmap="jet",
interpolation='none',
vmin=12,
vmax=16)
self.ax.grid(None)
self.ax.set_ylabel("Freq. [Hz]",
fontsize=font_size,
fontname='serif')
self.ax.set_yticks(yticks)
self.ax.set_yticklabels(["$%.1f$" % y for y in yticks_label],
fontsize=font_size)
self.ax.set_xticks(xticks)
self.ax.set_xticklabels(["$%.1f$" % x for x in xticks_label],
fontsize=font_size)
self.ax.set_xlabel("Time [s]",
fontsize=font_size,
fontname='serif')
if self.colorbarflag == 0:
self.colorbarflag = 1
self.cb = self.fig.colorbar(self.im)
elif self.colorbarflag == 1:
self.cb.update_bruteforce(self.im)
self.canvas.draw()
self.cnt_fig += 1
QApplication.processEvents()
def update_subscriber(self, topic_name):
self.topic_name = topic_name
self.le_topic.setText(self.topic_name)
if hasattr(self, 'sub'):
self.sub.unregister()
self.spectrogram = None
#self.topic_name = 'spectrum'
self.sub = rospy.Subscriber(topic_name,
Spectrum,
self.spectrum_callback,
queue_size=5)
def shutdown_plugin(self):
pass