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 __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.scrollable = QWidget()
self.gridlayout = QGridLayout()
self.scroll = QScrollArea()
self.scroll.setWidgetResizable(True)
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
joint_layout.addWidget(slider)
self.joint_map[name] = {
'slidervalue': 0,
'display': display,
'slider': slider,
'joint': joint
}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(
lambda event, name=name: self.onValueChangedOne(name))
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders),
self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.scrollable.setLayout(self.gridlayout)
self.scroll.setWidget(self.scrollable)
self.vlayout.addWidget(self.scroll)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
self.setLayout(self.vlayout)
class JointStatePublisherGui(QWidget):
sliderUpdateTrigger = Signal()
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.scrollable = QWidget()
self.gridlayout = QGridLayout()
self.scroll = QScrollArea()
self.scroll.setWidgetResizable(True)
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
joint_layout.addWidget(slider)
self.joint_map[name] = {
'slidervalue': 0,
'display': display,
'slider': slider,
'joint': joint
}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(
lambda event, name=name: self.onValueChangedOne(name))
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders),
self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.scrollable.setLayout(self.gridlayout)
self.scroll.setWidget(self.scrollable)
self.vlayout.addWidget(self.scroll)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
self.setLayout(self.vlayout)
def onValueChangedOne(self, name):
# A slider value was changed, but we need to change the joint_info metadata.
joint_info = self.joint_map[name]
joint_info['slidervalue'] = joint_info['slider'].value()
joint = joint_info['joint']
joint['position'] = self.sliderToValue(joint_info['slidervalue'],
joint)
joint_info['display'].setText("%.2f" % joint['position'])
@pyqtSlot()
def updateSliders(self):
self.update_sliders()
def update_sliders(self):
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(
joint['position'], joint)
joint_info['slider'].setValue(joint_info['slidervalue'])
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(joint['zero'], joint))
def reorggrid_event(self, event):
self.reorganize_grid(event)
def reorganize_grid(self, number_of_rows):
self.num_rows = number_of_rows
# Remove items from layout (won't destroy them!)
items = []
for pos in self.positions:
item = self.gridlayout.itemAtPosition(*pos)
items.append(item)
self.gridlayout.removeItem(item)
# Generate new positions for sliders and place them in their new spots
self.positions = self.generate_grid_positions(len(items),
self.num_rows)
for item, pos in zip(items, self.positions):
self.gridlayout.addLayout(item, *pos)
def generate_grid_positions(self, num_items, num_rows):
if num_rows == 0:
return []
positions = [
(y, x)
for x in range(int((math.ceil(float(num_items) / num_rows))))
for y in range(num_rows)
]
positions = positions[:num_items]
return positions
def randomize_event(self, event):
self.randomize()
def randomize(self):
rospy.loginfo("Randomizing")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(random.uniform(joint['min'], joint['max']),
joint))
def sliderUpdate(self, event):
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
self.update_sliders()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] -
joint['min'])
def sliderToValue(self, slider, joint):
pctvalue = slider / float(RANGE)
return joint['min'] + (joint['max'] - joint['min']) * pctvalue
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)
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.gridlayout = QGridLayout()
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE/2)
joint_layout.addWidget(slider)
self.joint_map[name] = {'slidervalue': 0, 'display': display,
'slider': slider, 'joint': joint}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(self.onValueChanged)
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders), self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.vlayout.addLayout(self.gridlayout)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.lineEdit().setReadOnly(True) # don't edit it by hand to avoid weird resizing of window
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
class JointStatePublisherGui(QWidget):
sliderUpdateTrigger = Signal()
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.gridlayout = QGridLayout()
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE/2)
joint_layout.addWidget(slider)
self.joint_map[name] = {'slidervalue': 0, 'display': display,
'slider': slider, 'joint': joint}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(self.onValueChanged)
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders), self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.vlayout.addLayout(self.gridlayout)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.lineEdit().setReadOnly(True) # don't edit it by hand to avoid weird resizing of window
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
@pyqtSlot(int)
def onValueChanged(self, event):
# A slider value was changed, but we need to change the joint_info metadata.
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
joint = joint_info['joint']
joint['position'] = self.sliderToValue(joint_info['slidervalue'], joint)
joint_info['display'].setText("%.2f" % joint['position'])
@pyqtSlot()
def updateSliders(self):
self.update_sliders()
def update_sliders(self):
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(joint['position'],
joint)
joint_info['slider'].setValue(joint_info['slidervalue'])
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(self.valueToSlider(joint['zero'], joint))
def reorggrid_event(self, event):
self.reorganize_grid(event)
def reorganize_grid(self, number_of_rows):
self.num_rows = number_of_rows
# Remove items from layout (won't destroy them!)
items = []
for pos in self.positions:
item = self.gridlayout.itemAtPosition(*pos)
items.append(item)
self.gridlayout.removeItem(item)
# Generate new positions for sliders and place them in their new spots
self.positions = self.generate_grid_positions(len(items), self.num_rows)
for item, pos in zip(items, self.positions):
self.gridlayout.addLayout(item, *pos)
def generate_grid_positions(self, num_items, num_rows):
if num_rows==0:
return []
positions = [(y, x) for x in range(int((math.ceil(float(num_items) / num_rows)))) for y in range(num_rows)]
positions = positions[:num_items]
return positions
def randomize_event(self, event):
self.randomize()
def randomize(self):
rospy.loginfo("Randomizing")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(random.uniform(joint['min'], joint['max']), joint))
def sliderUpdate(self, event):
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
self.update_sliders()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] - joint['min'])
def sliderToValue(self, slider, joint):
pctvalue = slider / float(RANGE)
return joint['min'] + (joint['max']-joint['min']) * pctvalue
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 Supervision(Plugin):
StepByStepParam = "/sm_sot_hpp/step_by_step"
StepTopic = "/sm_sot_hpp/step"
PathExecutionTopic = "/sm_sot_hpp/start_path"
PublishStateService = "/sot/publish_state"
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)
def publishState(self):
try:
rospy.wait_for_service(Supervision.PublishStateService, 0.5)
publish_state = rospy.ServiceProxy(Supervision.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
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
