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