def __init__(self, parent=None, current_values=None):
super(BlacklistDialog, self).__init__(parent)
self.setWindowTitle("Blacklist")
vbox = QVBoxLayout()
self.setLayout(vbox)
self._blacklist = Blacklist()
if isinstance(current_values, list):
for val in current_values:
self._blacklist.append(val)
vbox.addWidget(self._blacklist)
controls_layout = QHBoxLayout()
add_button = QPushButton(icon=QIcon.fromTheme('list-add'))
rem_button = QPushButton(icon=QIcon.fromTheme('list-remove'))
ok_button = QPushButton("Ok")
cancel_button = QPushButton("Cancel")
add_button.clicked.connect(self._add_item)
rem_button.clicked.connect(self._remove_item)
ok_button.clicked.connect(self.accept)
cancel_button.clicked.connect(self.reject)
controls_layout.addWidget(add_button)
controls_layout.addWidget(rem_button)
controls_layout.addStretch(0)
controls_layout.addWidget(ok_button)
controls_layout.addWidget(cancel_button)
vbox.addLayout(controls_layout)
def setup_group_frame(self, group):
layout = QVBoxLayout()
# grid for buttons for named targets
grid = QGridLayout()
grid.setSpacing(1)
self.button_group = QButtonGroup(self)
row = 0
column = 0
named_targets = self.get_named_targets(group)
for target in named_targets:
button = QPushButton(target)
self.button_group.addButton(button)
grid.addWidget(button, row, column)
column += 1
if column >= self.MAX_COLUMNS:
row += 1
column = 0
self.button_group.buttonClicked.connect(self._handle_button_clicked)
# grid for show joint value and move arm buttons/text field
joint_values_grid = QGridLayout()
joint_values_grid.setSpacing(1)
button_show_joint_values = QPushButton('Current Joint Values')
button_show_joint_values.clicked[bool].connect(
self._handle_show_joint_values_clicked)
line_edit = QLineEdit()
self.text_joint_values.append(line_edit)
button_move_to = QPushButton('Move to')
button_move_to.clicked[bool].connect(self._handle_move_to_clicked)
joint_values_grid.addWidget(button_show_joint_values, 0, 1)
joint_values_grid.addWidget(line_edit, 0, 2)
joint_values_grid.addWidget(button_move_to, 0, 3)
layout.addLayout(grid)
line = QFrame()
line.setFrameShape(QFrame.HLine)
line.setFrameShadow(QFrame.Sunken)
layout.addWidget(line)
layout.addLayout(joint_values_grid)
frame = QFrame()
frame.setLayout(layout)
return frame
def create_progressbar(self, aircraft_no):
waypoint_layout = QVBoxLayout()
waypoint_layout.setContentsMargins(0, 10, 0, 10)
waypoint_header_layout = QHBoxLayout(
) # waypoint_header_layout will be nested inside waypoint_layout with the progress bar beneath it
aircraft_label = QLabel('Aircraft ' + str(aircraft_no))
self.waypoint_plaintext_dict['aircraft' +
str(aircraft_no)] = QPlainTextEdit()
self.waypoint_plaintext_dict.get('aircraft' +
str(aircraft_no)).setMaximumHeight(40)
self.waypoint_plaintext_dict.get('aircraft' +
str(aircraft_no)).setReadOnly(True)
self.waypoint_plaintext_dict[
'aircraftlink' + str(aircraft_no)] = QPlainTextEdit("Telegram")
self.waypoint_plaintext_dict.get('aircraftlink' +
str(aircraft_no)).setMaximumHeight(40)
self.waypoint_plaintext_dict.get('aircraftlink' +
str(aircraft_no)).setMaximumWidth(100)
self.waypoint_plaintext_dict.get('aircraftlink' +
str(aircraft_no)).setReadOnly(True)
self.waypoint_plaintext_dict['progress_bar_aircraft' +
str(aircraft_no)] = QProgressBar()
waypoint_header_layout.addWidget(aircraft_label)
waypoint_header_layout.addWidget(
self.waypoint_plaintext_dict['aircraft' + str(aircraft_no)])
waypoint_header_layout.addWidget(
self.waypoint_plaintext_dict['aircraftlink' + str(aircraft_no)])
waypoint_layout.addLayout(waypoint_header_layout)
waypoint_layout.addWidget(
self.waypoint_plaintext_dict['progress_bar_aircraft' +
str(aircraft_no)])
self.progressbar_layout.addLayout(waypoint_layout)
def __init__(self, parent=None):
super(VisualizerWidget, self).__init__(parent)
self.setWindowTitle('Graph Profiler Visualizer')
vbox = QVBoxLayout()
self.setLayout(vbox)
toolbar_layout = QHBoxLayout()
refresh_button = QPushButton()
refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
auto_refresh_checkbox = QCheckBox("Auto Refresh")
hide_disconnected_topics = QCheckBox("Hide Disconnected Topics")
topic_blacklist_button = QPushButton("Topic Blacklist")
node_blacklist_button = QPushButton("Node Blacklist")
refresh_button.clicked.connect(self._refresh)
topic_blacklist_button.clicked.connect(self._edit_topic_blacklist)
node_blacklist_button.clicked.connect(self._edit_node_blacklist)
auto_refresh_checkbox.setCheckState(2)
auto_refresh_checkbox.stateChanged.connect(self._autorefresh_changed)
hide_disconnected_topics.setCheckState(2)
hide_disconnected_topics.stateChanged.connect(
self._hidedisconnectedtopics_changed)
toolbar_layout.addWidget(refresh_button)
toolbar_layout.addWidget(auto_refresh_checkbox)
toolbar_layout.addStretch(0)
toolbar_layout.addWidget(hide_disconnected_topics)
toolbar_layout.addWidget(topic_blacklist_button)
toolbar_layout.addWidget(node_blacklist_button)
vbox.addLayout(toolbar_layout)
# Initialize the Visualizer
self._view = qt_view.QtView()
self._adapter = rosprofiler_adapter.ROSProfileAdapter(self._view)
self._adapter.set_topic_quiet_list(TOPIC_BLACKLIST)
self._adapter.set_node_quiet_list(NODE_BLACKLIST)
vbox.addWidget(self._view)
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_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_sliders(self):
sliderbox = self._widget.findChild(QLayout, 'Sliders')
firstCol = QVBoxLayout()
graph_button = QPushButton()
graph_button.setCheckable(True)
graph_button.setText("Graph Off")
graph_button.toggle()
graph_button.clicked.connect(self.set_graph_state)
reset_button = QPushButton()
reset_button.setCheckable(False)
reset_button.setText("Reset")
reset_button.clicked.connect(self.set_reset)
self.graph_button = graph_button
self.reset_button = reset_button
firstCol.addWidget(graph_button)
firstCol.addWidget(reset_button)
firstCol.setAlignment(graph_button, Qt.AlignVCenter)
firstCol.setAlignment(reset_button, Qt.AlignVCenter)
zero_button = QPushButton()
zero_button.setCheckable(False)
zero_button.setText("Set All Zero")
zero_button.clicked.connect(self.set_pressure_zero)
self.zero_button = zero_button
firstCol.addWidget(zero_button)
firstCol.setAlignment(zero_button, Qt.AlignVCenter)
transition_box = QVBoxLayout()
label = QLabel()
label.setAlignment(Qt.AlignCenter)
label.setText("Transition Time")
spinbox = QDoubleSpinBox()
spinbox.setMinimum(0)
spinbox.setMaximum(10)
spinbox.setValue(self.settings['transitions'])
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
spinbox.setSuffix(" sec")
spinbox.valueChanged.connect(self.set_transition_value)
transition_box.addWidget(label)
transition_box.addWidget(spinbox)
transition_box.setAlignment(label, Qt.AlignBottom)
transition_box.setAlignment(spinbox, Qt.AlignTop)
firstCol.addLayout(transition_box)
self.sliders = []
sliderbox.addLayout(firstCol)
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': None, 'number': None, 'on_off': None}
layout_cluster = QVBoxLayout()
layout = QVBoxLayout()
layout.setAlignment(Qt.AlignHCenter)
slider = QSlider(Qt.Vertical)
slider.setMinimum(self.settings['min_pressure'][idx] * 10.0)
slider.setMaximum(self.settings['max_pressure'][idx] * 10.0)
slider.setValue(0)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(20)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(self.settings['min_pressure'][idx])
spinbox.setMaximum(self.settings['max_pressure'][idx])
spinbox.setValue(0)
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
cb_function_curr = lambda value, idx=idx, slider=False: self.send_slider_value(
idx, value, slider)
cb_function_curr2 = lambda value, idx=idx, slider=True: self.send_slider_value(
idx, value, slider)
slider.valueChanged.connect(cb_function_curr2)
spinbox.valueChanged.connect(cb_function_curr)
labelmax = QLabel()
labelmax.setAlignment(Qt.AlignCenter)
labelmax.setText("%0.1f" %
(self.settings['max_pressure'][idx]))
labelmin = QLabel()
labelmin.setAlignment(Qt.AlignCenter)
labelmin.setText("%0.1f" %
(self.settings['min_pressure'][idx]))
layout.addWidget(labelmax)
layout.addWidget(slider)
layout.addWidget(labelmin)
layout.addWidget(spinbox)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
label = QLabel()
label.setText("Chan. %d" % (chan_idx + 1))
label.setAlignment(Qt.AlignCenter)
layout_cluster.addWidget(label)
layout_cluster.addLayout(layout)
slider_group['slider'] = slider
slider_group['number'] = spinbox
on_button = QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][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.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_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_i': None,
'number_i': None,
'slider_d': None,
'number_d': 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(3):
layout = QVBoxLayout()
layout.setAlignment(Qt.AlignHCenter)
if j == 0:
maxrange = 1.0
elif j == 1:
maxrange = 10
elif j == 2:
maxrange = 10
slider = QSlider(Qt.Vertical)
slider.setMinimum(0)
slider.setMaximum(maxrange * 100)
slider.setValue(self.settings['pid_gains'][chan_idx][j] *
100)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(maxrange / 100.0)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(0)
spinbox.setMaximum(maxrange)
spinbox.setValue(self.settings['pid_gains'][chan_idx][j])
spinbox.setDecimals(2)
spinbox.setSingleStep(maxrange / 100.0)
cb_function_number = lambda value, idx=idx, gain_idx=j, slider=False: self.send_slider_value(
idx, gain_idx, value, slider)
cb_function_slider = lambda value, idx=idx, gain_idx=j, slider=True: self.send_slider_value(
idx, gain_idx, value, slider)
slider.valueChanged.connect(cb_function_slider)
spinbox.valueChanged.connect(cb_function_number)
label = QLabel()
label.setAlignment(Qt.AlignCenter)
if j == 0:
slider_group['slider_p'] = slider
slider_group['number_p'] = spinbox
label.setText("P")
elif j == 1:
slider_group['slider_i'] = slider
slider_group['number_i'] = spinbox
label.setText("I")
elif j == 2:
slider_group['slider_d'] = slider
slider_group['number_d'] = spinbox
label.setText("D")
labelmax = QLabel()
labelmax.setAlignment(Qt.AlignCenter)
labelmax.setText("%0.1f" % (maxrange))
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 NavViewWidget(QWidget):
def __init__(self, map_topic='/map',
paths=['/move_base/NavFn/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons=['/move_base/local_costmap/robot_footprint']):
super(NavViewWidget, self).__init__()
self._layout = QVBoxLayout()
self._button_layout = QHBoxLayout()
self.setAcceptDrops(True)
self.setWindowTitle('Navigation Viewer')
self.paths = paths
self.polygons = polygons
self.map = map_topic
self._tf = tf.TransformListener()
self._nav_view = NavView(map_topic, paths, polygons, tf = self._tf, parent = self)
self._set_pose = QPushButton('Set Pose')
self._set_pose.clicked.connect(self._nav_view.pose_mode)
self._set_goal = QPushButton('Set Goal')
self._set_goal.clicked.connect(self._nav_view.goal_mode)
self._button_layout.addWidget(self._set_pose)
self._button_layout.addWidget(self._set_goal)
self._layout.addLayout(self._button_layout)
self._layout.addWidget(self._nav_view)
self.setLayout(self._layout)
def dragEnterEvent(self, e):
if not e.mimeData().hasText():
if not hasattr(e.source(), 'selectedItems') or len(e.source().selectedItems()) == 0:
qWarning('NavView.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
return
item = e.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name == None:
qWarning('NavView.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
return
else:
topic_name = str(e.mimeData().text())
if accepted_topic(topic_name):
e.accept()
e.acceptProposedAction()
def dropEvent(self, e):
if e.mimeData().hasText():
topic_name = str(e.mimeData().text())
else:
droped_item = e.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
topic_type, array = get_field_type(topic_name)
if not array:
if topic_type is OccupancyGrid:
self.map = topic_name
# Swap out the nav view for one with the new topics
self._nav_view.close()
self._nav_view = NavView(self.map, self.paths, self.polygons, self._tf, self)
self._layout.addWidget(self._nav_view)
elif topic_type is Path:
self.paths.append(topic_name)
self._nav_view.add_path(topic_name)
elif topic_type is PolygonStamped:
self.polygons.append(topic_name)
self._nav_view.add_polygon(topic_name)
def save_settings(self, plugin_settings, instance_settings):
self._nav_view.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._nav_view.restore_settings(plugin_settings, instance_settings)
class ChecklistWindow(QWidget):
def __init__(self, aircraft_id):
super(ChecklistWindow, self).__init__()
# Set properties of the window
self.setWindowTitle("BTO and BPO Checklist")
self.resize(500, 700)
self.move(200,100)
self.checklist_state = 0
# Relative path for the default BPO and BTO checklist
BPO_checklist_file = os.path.join(rospkg.RosPack().get_path('yonah_rqt'), 'src/yonah_rqt', 'BPO_checklist.csv')
BTO_checklist_file = os.path.join(rospkg.RosPack().get_path('yonah_rqt'), 'src/yonah_rqt', 'BTO_checklist.csv')
# Check whether checklist is present, if not print a error message to terminal
try:
# If checklist is present, parse it and pass it to its respective variable
self.BPO_checklist = self.excel_parser(BPO_checklist_file)
self.BTO_checklist = self.excel_parser(BTO_checklist_file)
except:
rospy.logerr("Checklist files are missing or named wrongly. Please follow the original directory and naming")
exit()
# Create the layout
self.main_layout = QVBoxLayout()
self.buttons_layout = QHBoxLayout()
self.tree_widget_layout = QHBoxLayout()
# Create the widgets
self.create_widget()
self.has_message_opened = 0
# Add the widgets into the layouts
self.main_layout.addLayout(self.tree_widget_layout)
self.main_layout.addLayout(self.buttons_layout)
self.setLayout(self.main_layout)
# Create the main layout of widget
def create_widget(self):
# Create tree structure
self.create_tree()
# Declare buttons and connect each of them to a function
self.load_button = QPushButton('Load')
self.ok_button = QPushButton('OK')
self.cancel_button = QPushButton('Cancel')
self.load_button.pressed.connect(self.load_clicked)
self.ok_button.pressed.connect(self.ok_clicked)
self.cancel_button.pressed.connect(self.cancel_clicked)
# Add buttons into the layout
self.buttons_layout.addWidget(self.load_button)
self.buttons_layout.addWidget(self.cancel_button)
self.buttons_layout.addWidget(self.ok_button)
self.tree_widget_layout.addWidget(self.tree_widget)
# Create the tree layout of widget inside of the main layout
def create_tree(self):
# Set up the main tree widget
self.tree_widget = QTreeWidget()
self.tree_widget.setColumnCount(2)
self.tree_widget.setColumnWidth(0, 250)
self.tree_widget.setHeaderLabels(['Parts', 'Status'])
self.item = QTreeWidgetItem()
# Create the BPO section
self.BPO_header = QTreeWidgetItem(self.tree_widget)
self.BPO_header.setFlags(self.BPO_header.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self.BPO_header.setText(0, 'BPO Checklist')
self.BPO_header.setExpanded(True)
self.create_item(self.BPO_header, self.BPO_checklist) # Adds the list of items into the section
# Create the BTO section
self.BTO_header = QTreeWidgetItem(self.tree_widget)
self.BTO_header.setFlags(self.BTO_header.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
self.BTO_header.setText(0, 'BTO Checklist')
self.BTO_header.setExpanded(True)
self.create_item(self.BTO_header, self.BTO_checklist) # Adds the list of items into the section
# Populate the tree layout with items
def create_item(self, parent, list):
section_header = [] # List of the section headers
for i in range (len(list)):
if (list[i][1] == '' and list[i][0] != ''):
section_header.append(list[i][0])
k = 0
# Iterate through the different sections
for j in range (len(section_header)):
# Child refers to the sections (mechanical, avionics, etc)
child = QTreeWidgetItem(parent)
child.setFlags(child.flags() | Qt.ItemIsTristate | Qt.ItemIsUserCheckable)
child.setText(0, section_header[j])
while k < len(list):
if list[k][0] in section_header:
# When the while loop encounters the first title, continue the loop
if (list[k][0] == section_header[0]):
k += 1
continue
# When the while loop encounters the next titles, break the loop so that the value of j increases
k += 1
break
# when the list contains empty cells, skip the cell and continue
elif list[k][0] == '':
k += 1
continue
# Add the list items to the treewidgetitem
# Grandchild refers to the items under each section (wing nuts, tail nuts, etc)
grandchild = QTreeWidgetItem(child)
grandchild.setText(0, list[k][0])
grandchild.setText(1, list[k][1])
grandchild.setCheckState(0, Qt.Unchecked) # Set all checkbox to its unchecked state
k += 1
# Read the excel sheet and parse it as an array
def excel_parser(self, file_name):
with open(file_name, 'r') as file:
checklist = []
reader = csv.reader(file)
for row in reader:
checklist.append(row)
return checklist
# Determines what happens when load button is clicked
def load_clicked(self):
# Use QFileDialog to open the system's file browser
filenames = QFileDialog.getOpenFileNames(
self, self.tr('Load from Files'), '.', self.tr('csv files {.csv} (*.csv)'))
# Iterate through the file names selected
for filename in filenames[0]:
# If the file names has the word BPO or BTO in it, remove current widget, add the loaded one
if (filename.find('BPO') != -1):
self.BPO_checklist = self.excel_parser(filename)
self.remove_widget()
self.create_widget()
elif (filename.find('BTO') != -1):
self.BTO_checklist = self.excel_parser(filename)
self.remove_widget()
self.create_widget()
else:
rospy.logerr('rqt: Checklist name must contain BPO or BTO')
self.close()
# Close all the main_layout
def remove_widget(self):
self.main_layout.removeWidget(self.tree_widget)
self.tree_widget.deleteLater()
self.buttons_layout.removeWidget(self.ok_button)
self.buttons_layout.removeWidget(self.cancel_button)
self.buttons_layout.removeWidget(self.load_button)
self.ok_button.deleteLater()
self.cancel_button.deleteLater()
self.load_button.deleteLater()
# Declare what will happen when ok button is clicked
def ok_clicked(self):
if self.BPO_header.checkState(0) != 2 or self.BTO_header.checkState(0) != 2: # User clicks ok without checking all
self.dialog_window("Some items in the checklist are still unchecked", "Do you still want to continue?", True)
else:
self.checklist_state = 1
self.close()
# Declare what will happen when cancel button is clicked
def cancel_clicked(self):
if self.BPO_header.checkState(0) != 0 or self.BTO_header.checkState(0) != 0: # User clicks cancel with some boxes checked
self.dialog_window('Some of your items are checked. Cancelling will uncheck all your items', 'Do you still want to continue?', False)
else:
self.BTO_header.setCheckState(0, Qt.Unchecked)
self.BPO_header.setCheckState(0, Qt.Unchecked)
self.close()
# Create a pop up window when user pre-emptively cancelled or clicked ok without completing the checklist
def dialog_window(self, message, detail, check):
self.message = QMessageBox()
self.has_message_opened = 1
self.message.setIcon(QMessageBox.Warning)
self.message.setText(message)
self.message.setInformativeText(detail)
self.message.setWindowTitle("Items are unchecked")
self.message.setStandardButtons(QMessageBox.Yes | QMessageBox.No)
self.message.show()
if check == True: # Check == True means it is from ok_clicked
self.message.buttonClicked.connect(self.message_action)
else:
self.message.buttonClicked.connect(self.message_action_uncheck)
# Determines what happens after dialog_window pops up from ok_button
def message_action(self, i):
if i.text() == '&Yes':
self.checklist_state = 1
self.close()
else:
self.message.close()
# Determines what happens after dialog_window pops up from cancel_button
def message_action_uncheck(self, i):
self.response = i.text()
if self.response == '&Yes':
self.checklist_state = 1
self.BTO_header.setCheckState(0, Qt.Unchecked)
self.BPO_header.setCheckState(0, Qt.Unchecked)
self.close()
else:
self.message.close()
# Shutdown function
def shutdown(self):
self.close()
if self.has_message_opened == 1:
self.message.close()
class NavViewWidget(QWidget):
def __init__(self, map_topic='/map', paths=None, polygons=None):
super(NavViewWidget, self).__init__()
if paths is None:
paths = [
'/move_base/NavFn/plan',
'/move_base/TrajectoryPlannerROS/local_plan'
]
if polygons is None:
polygons = ['/move_base/local_costmap/robot_footprint']
self._layout = QVBoxLayout()
self._button_layout = QHBoxLayout()
self.setAcceptDrops(True)
self.setWindowTitle('Navigation Viewer')
self.paths = paths
self.polygons = polygons
self.map_topic = map_topic
self._tf = tf.TransformListener()
self._set_pose = QPushButton('Set Pose')
self._set_goal = QPushButton('Set Goal')
self._button_layout.addWidget(self._set_pose)
self._button_layout.addWidget(self._set_goal)
self._layout.addLayout(self._button_layout)
self._nav_view = None
self.setLayout(self._layout)
def new_nav_view(self):
if self._nav_view:
self._nav_view.close()
self._nav_view = NavView(self.map_topic,
self.paths,
self.polygons,
tf_listener=self._tf,
parent=self)
self._set_pose.clicked.connect(self._nav_view.pose_mode)
self._set_goal.clicked.connect(self._nav_view.goal_mode)
self._layout.addWidget(self._nav_view)
def dragEnterEvent(self, e):
if not e.mimeData().hasText():
if not hasattr(e.source(), 'selectedItems') or len(
e.source().selectedItems()) == 0:
qWarning(
'NavView.dragEnterEvent(): not hasattr(event.source(), selectedItems) or '
'len(event.source().selectedItems()) == 0')
return
item = e.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name is None:
qWarning(
'NavView.dragEnterEvent(): not hasattr(item, ros_topic_name_)'
)
return
else:
topic_name = str(e.mimeData().text())
if accepted_topic(topic_name):
e.acceptProposedAction()
def dropEvent(self, e):
if e.mimeData().hasText():
topic_name = str(e.mimeData().text())
else:
dropped_item = e.source().selectedItems()[0]
topic_name = str(dropped_item.data(0, Qt.UserRole))
topic_type, array = get_field_type(topic_name)
if not array:
if topic_type is OccupancyGrid:
self.map_topic = topic_name
# Swap out the nav view for one with the new topics
self.new_nav_view()
elif topic_type is Path:
self.paths.append(topic_name)
self._nav_view.add_path(topic_name)
elif topic_type is PolygonStamped:
self.polygons.append(topic_name)
self._nav_view.add_polygon(topic_name)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value("map_topic", self.map_topic)
instance_settings.set_value("paths", self.paths)
instance_settings.set_value("polygons", self.polygons)
def restore_settings(self, plugin_settings, instance_settings):
try:
self.map_topic = instance_settings.value("map_topic", "/map")
except Exception:
pass
try:
self.paths = instance_settings.value("paths", [])
except Exception:
pass
try:
self.polygons = instance_settings.value("polygons", [])
except Exception:
pass
self.new_nav_view()
def trigger_configuration(self):
"""
Callback when the configuration button is clicked
"""
changed = False
map_topics = sorted(rostopic.find_by_type('nav_msgs/OccupancyGrid'))
try:
index = map_topics.index(self.map_topic)
except ValueError:
index = 0
map_topic, ok = QInputDialog.getItem(self, "Select map topic name",
"Topic name", map_topics, index)
if ok:
if map_topic != self.map_topic:
changed = True
self.map_topic = map_topic
# Paths
path_topics = sorted(rostopic.find_by_type('nav_msgs/Path'))
path_topics = [(topic, topic in self.paths) for topic in path_topics]
dialog = ListDialog("Select path topic(s)", path_topics, self)
paths, ok = dialog.exec_()
if ok:
if not paths:
changed = True
diff = set(paths).symmetric_difference(set(self.paths))
if diff:
self.paths = paths
changed = True
# Polygons
polygon_topics = sorted(
rostopic.find_by_type('geometry_msgs/PolygonStamped'))
polygon_topics = [(topic, topic in self.polygons)
for topic in polygon_topics]
dialog = ListDialog("Select polygon topic(s)", polygon_topics, self)
polygons, ok = dialog.exec_()
if ok:
if not polygons:
changed = True
diff = set(polygons).symmetric_difference(set(self.polygons))
if diff:
self.polygons = polygons
changed = True
if changed:
rospy.logdebug(
"New configuration is different, creating a new nav_view")
self.new_nav_view()
def init_sliders(self):
sliderbox = self._widget.findChild(QLayout,'Sliders')
firstCol = QVBoxLayout()
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
reset_button=QPushButton()
reset_button.setCheckable(False)
reset_button.setText("Reset")
reset_button.clicked.connect(self.set_reset)
self.graph_button = graph_button
self.reset_button = reset_button
firstCol.addWidget(graph_button)
firstCol.addWidget(reset_button)
firstCol.addWidget(graph_button)
firstCol.setAlignment(graph_button,Qt.AlignVCenter)
zero_button=QPushButton()
zero_button.setCheckable(False)
zero_button.setText("Set All Zero")
zero_button.clicked.connect(self.set_pressure_zero)
self.zero_button = zero_button
firstCol.addWidget(zero_button)
firstCol.setAlignment(zero_button,Qt.AlignVCenter)
transition_box = QVBoxLayout()
label = QLabel()
label.setAlignment(Qt.AlignCenter)
label.setText("Transition Time")
spinbox = QDoubleSpinBox()
spinbox.setMinimum(0)
spinbox.setMaximum(10)
spinbox.setValue(self.settings['transitions'])
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
spinbox.setSuffix(" sec")
spinbox.valueChanged.connect(self.set_transition_value)
transition_box.addWidget(label)
transition_box.addWidget(spinbox)
transition_box.setAlignment(label,Qt.AlignBottom)
transition_box.setAlignment(spinbox,Qt.AlignTop)
firstCol.addLayout(transition_box)
self.sliders = []
sliderbox.addLayout(firstCol)
all_rows_layout = QVBoxLayout()
g_idx = 0
for row in self.settings['gui_config']:
num_groups_row = len(row)
row_layout = QHBoxLayout()
for gr_idx, s_group in enumerate(row):
g_channels = s_group['channels']
g_layout = s_group['layout']
if 'horiz' in g_layout:
group_layout = QHBoxLayout()
else:
group_layout = QVBoxLayout()
control_group = {'sliders': [], 'on_off': None}
label = QLabel()
label.setText("Group. %d"%(g_idx+1))
label.setAlignment(Qt.AlignCenter)
group_layout.addWidget(label)
for c_idx, s_idx in enumerate(g_channels):
idx = s_idx*1
slider_group={'slider':None, 'number':None}
layout_cluster = QVBoxLayout()
labelfirst = QLabel()
labelfirst.setAlignment(Qt.AlignCenter)
labellast = QLabel()
labellast.setAlignment(Qt.AlignCenter)
layout = QVBoxLayout()
if 'diff' in g_layout and c_idx == 0:
sublayout=QHBoxLayout()
layout.setAlignment(Qt.AlignVCenter)
slider = QSlider(Qt.Horizontal)
slider.setMinimum(-100)
slider.setMaximum(100)
slider.setValue(0)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(20)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(-10)
spinbox.setMaximum(10)
spinbox.setValue(0)
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
labellast.setText("%0.1f"%(10)) # These are flipped becasue of order
labelfirst.setText("%0.1f"%(-10))
max_label = labellast
min_label = labelfirst
else:
layout.setAlignment(Qt.AlignHCenter)
slider = QSlider(Qt.Vertical)
slider.setMinimum(self.settings['min_pressure'][idx]*10.0)
slider.setMaximum(self.settings['max_pressure'][idx]*10.0)
slider.setValue(0)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(20)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(self.settings['min_pressure'][idx])
spinbox.setMaximum(self.settings['max_pressure'][idx])
spinbox.setValue(0)
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
labelfirst.setText("%0.1f"%(self.settings['max_pressure'][idx]))
labellast.setText("%0.1f"%(self.settings['min_pressure'][idx]))
max_label = labelfirst
min_label = labellast
cb_function_number = lambda value, g_idx=g_idx, s_idx=c_idx, slider=False: self.send_slider_value(g_idx,s_idx,value,slider)
cb_function_slider = lambda value, g_idx=g_idx, s_idx=c_idx, slider=True: self.send_slider_value(g_idx,s_idx,value,slider)
slider.valueChanged.connect(cb_function_slider)
spinbox.valueChanged.connect(cb_function_number)
if 'diff' in g_layout and c_idx == 0:
sublayout.addWidget(labelfirst)
sublayout.addWidget(slider)
sublayout.addWidget(labellast)
layout.addWidget(spinbox)
layout.addLayout(sublayout)
else:
layout.addWidget(labelfirst)
layout.addWidget(slider)
layout.addWidget(labellast)
layout.addWidget(spinbox)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
layout_cluster.addLayout(layout)
slider_group['slider'] = slider
slider_group['number'] = spinbox
slider_group['max_label'] = max_label
slider_group['min_label'] = min_label
control_group['sliders'].append(slider_group)
group_layout.addLayout(layout_cluster)
on_button=QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][idx]:
on_button.toggle()
on_button.setText("On")
on_button.clicked.connect(lambda state, g_idx=g_idx: self.send_channel_state(g_idx,state))
group_layout.addWidget(on_button)
row_layout.addLayout(group_layout)
row_layout.addSpacing(20)
control_group['on_off'] = on_button
self.sliders.append(control_group)
g_idx+=1
all_rows_layout.addLayout(row_layout)
sliderbox.addLayout(all_rows_layout)
class SystemWidget(QWidget):
"""
main class inherits from the ui window class.
You can specify the topics that the topic pane.
SystemWidget.start must be called in order to update topic pane.
"""
_column_names = ['topic', 'type', 'bandwidth', 'rate', 'value']
def __init__(self, plugin=None):
"""
"""
super(SystemWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_agent'), 'resource',
'SystemWidget.ui')
loadUi(ui_file, self)
self._plugin = plugin
self._subsystems = {}
self.all_subsystems = {}
self._widgets = {}
self.prev_subsystems = []
self.levels_layouts = []
self.structure_root = None
self.structure_graph = None
self.structure_changed = False
self.scrollArea = QScrollArea()
self.scrollArea.setWidgetResizable(True)
self.horizontalLayout.addWidget(self.scrollArea)
self.mainWidget = QWidget()
self.scrollArea.setWidget(self.mainWidget)
self.verticalLayout = QVBoxLayout()
self.mainWidget.setLayout(self.verticalLayout)
# 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 checkStructureChange(self):
result = False
for subsystem_name in self.prev_subsystems:
if not subsystem_name in self._widgets:
result = True
break
if result == False:
for subsystem_name in self._widgets:
if not subsystem_name in self.prev_subsystems:
result = True
break
self.prev_subsystems = []
for subsystem_name in self._widgets:
self.prev_subsystems.append(subsystem_name)
return result
def start(self):
"""
This method needs to be called to start updating topic pane.
"""
self._timer_refresh_topics.start(100)
def generateStructure(self):
names = []
for w1_name in self._widgets:
names.append(w1_name)
self._widgets[w1_name].resetBuffersLayout()
for w1_name in self._widgets:
w1 = self._widgets[w1_name]
if not w1.isInitialized():
continue
for w2_name in self._widgets:
w2 = self._widgets[w2_name]
if not w2.isInitialized():
continue
common_buffers = w1.getCommonBuffers(w2)
if common_buffers != None:
w1.groupBuffers(common_buffers, w2.subsystem_name)
w2.groupBuffers(common_buffers, w1.subsystem_name)
parents_dict = {}
for w1_name in self._widgets:
w1 = self._widgets[w1_name]
if not w1.isInitialized():
continue
for w2_name in self._widgets:
w2 = self._widgets[w2_name]
if not w2.isInitialized():
continue
rel_pose = w1.getLowerSubsystemPosition(w2.subsystem_name)
if rel_pose != -1:
parents_dict[w2_name] = w1_name
# print parents_dict
# get top-most subsystem (root)
root = None
if parents_dict:
root = list(parents_dict.keys())[0]
while root in parents_dict:
root = parents_dict[root]
levels = []
if root == None:
# there are no levels
one_level = []
for w_name in self._widgets:
one_level.append(w_name)
if len(one_level) > 0:
levels.append(one_level)
else:
levels.append([root])
while True:
# expand all subsystems in the lowest level
current_lowest = levels[-1]
next_lower_level = []
for s in current_lowest:
lower_list = self._widgets[s].getLowerSubsystems()
next_lower_level = next_lower_level + lower_list
if len(next_lower_level) == 0:
break
else:
levels.append(next_lower_level)
# TODO: manage disjoint trees
added_subsystems = []
for l in levels:
for s in l:
added_subsystems.append(s)
for w_name in self._widgets:
if not w_name in added_subsystems:
print "WARNING: subsystem %s is not in the main tree. This is not implemented." % (
w_name)
print "levels:", levels
return levels
def layout_widgets(self, layout):
return (layout.itemAt(i) for i in range(layout.count()))
@Slot()
def refresh_topics(self):
"""
refresh tree view items
"""
#
# update the list of subsystems
#
topic_list = rospy.get_published_topics()
if topic_list is None:
rospy.logerr(
'Not even a single published topic found. Check network configuration'
)
return
# start new topic dict
new_subsystems = {}
for topic_name, topic_type in topic_list:
name_split = topic_name.split('/')
# print name_split
if (len(name_split) == 3) and (name_split[0] == '') and (
name_split[2]
== 'diag') and (topic_type
== "diagnostic_msgs/DiagnosticArray"):
subsystem_name = name_split[1]
# if topic is new
if subsystem_name not in self._subsystems:
# create new TopicInfo
topic_info = TopicInfo(topic_name, topic_type)
new_subsystems[subsystem_name] = topic_info
topic_info.start_monitoring()
else:
# if topic has been seen before, copy it to new dict and
# remove it from the old one
new_subsystems[subsystem_name] = self._subsystems[
subsystem_name]
del self._subsystems[subsystem_name]
# remove unused subsystems
while True:
repeat = False
for s in self._subsystems:
if not s in new_subsystems:
del self._subsystems[s]
repeat = True
break
if not repeat:
break
# switch to new topic dict
self._subsystems = new_subsystems
# print self._subsystems.keys()
#
# update each subsystem
#
new_widgets = {}
for subsystem_name in self._subsystems:
msg = self._subsystems[subsystem_name].last_message
if (msg != None) and (len(msg.status) == 2) and \
msg.status[0].name == 'components' and msg.status[1].name == 'diagnostics':
name_split = subsystem_name.split('/')
if not subsystem_name in self.all_subsystems:
self.all_subsystems[subsystem_name] = SubsystemWidget(
self._plugin, subsystem_name)
if not subsystem_name in self._widgets:
new_widgets[subsystem_name] = self.all_subsystems[
subsystem_name]
# self.verticalLayout.addWidget(new_widgets[subsystem_name])
else:
new_widgets[subsystem_name] = self._widgets[subsystem_name]
# del self._widgets[subsystem_name]
# for value in msg.status[1].values:
# if value.key == 'master_component':
# new_widgets[subsystem_name].setStateName(value.value, '')
# break
# remove unused subsystems
# while True:
# repeat = False
# for s in self._widgets:
# if not s in new_widgets:
# del self._widgets[s]
# repeat = True
# break
# if not repeat:
# break
self._widgets = new_widgets
# print self._widgets.keys()
structure_changed = self.checkStructureChange()
if structure_changed:
self.structure_changed = True
if self.structure_changed:
allInitialized = True
for subsystem_name in self._widgets:
if not self._widgets[subsystem_name].isInitialized():
allInitialized = False
break
if allInitialized:
# remove all widgets from layouts
# and remove all layouts
for i in reversed(range(len(self.levels_layouts))):
layout = self.levels_layouts[i]
for i in reversed(range(layout.count())):
# The new widget is deleted when its parent is deleted.
layout.itemAt(i).widget().setParent(None)
self.verticalLayout.removeItem(layout)
del layout
self.levels_layouts = []
levels = self.generateStructure()
for l in levels:
hbox = QHBoxLayout()
for w in l:
hbox.addWidget(self._widgets[w])
self._widgets[w].show()
self.levels_layouts.append(hbox)
self.verticalLayout.addLayout(hbox)
# for
# TODO
self.structure_changed = False
while True:
repeat = False
for s in self.all_subsystems:
if not s in self._widgets:
del self.all_subsystems[s]
repeat = True
break
if not repeat:
break
for subsystem_name in self._widgets:
self._widgets[subsystem_name].update_subsystem(
self._subsystems[subsystem_name].last_message)
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_topic: Failed to restore header state.")
class block_pick_and_place(QWidget):
def __init__(self, title="block_pick_and_place"):
super(block_pick_and_place, self).__init__()
# Initial
self.br = tf.TransformBroadcaster()
self.tf_listener = tf.TransformListener()
self.ik = [0, 0, 0, 0]
# Thread lock
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("arm")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=1)
self.gripper_cmd = rospy.Publisher('/gripper_joint/command',
Float64,
queue_size=1)
# safe shutdown
rospy.on_shutdown(self.onShutdown)
# timer
rospy.loginfo("[%s] Initialized " % (rospy.get_name()))
self.group.allow_replanning(True)
self.group.set_pose_reference_frame("base_link")
self.group.set_goal_position_tolerance(0.005)
self.group.set_goal_orientation_tolerance(0.05)
self.home_pose()
self.init_interactive_marker()
self.arm_plan_state = False
self.vlayout = QVBoxLayout(self)
self.gridlayout = QGridLayout()
self.vlayout.addLayout(self.gridlayout)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.CWbutton = QPushButton('Start_to_plan', self)
self.CWbutton.clicked.connect(self.start_event)
self.vlayout.addWidget(self.CWbutton)
self.SPbutton = QPushButton('Stop', self)
self.SPbutton.clicked.connect(self.stop_event)
self.vlayout.addWidget(self.SPbutton)
def start_event(self, event):
rospy.loginfo('ARM: START')
self.arm_plan_state = True
self.start_ik(self.ik[0], self.ik[1], self.ik[2], self.ik[3])
def stop_event(self, event):
rospy.loginfo('ARM: STOP ')
self.arm_plan_state = False
# print self.waypoints
def init_interactive_marker(self):
server = InteractiveMarkerServer("simple_marker")
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/world"
int_marker.name = "Object"
int_marker.description = "Object Control"
int_marker.scale = 0.1
int_marker.pose.position.x = 0.3
int_marker.pose.position.z = 0.01
# create a grey box marker
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.scale.x = 0.02
box_marker.scale.y = 0.02
box_marker.scale.z = 0.02
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
# create a non-interactive control which contains the box
box_control = InteractiveMarkerControl()
box_control.always_visible = True
box_control.markers.append(box_marker)
# add the control to the interactive marker
int_marker.controls.append(box_control)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
rotate_control = InteractiveMarkerControl()
rotate_control.name = "move_3D"
rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_3D
# add the control to the interactive marker
int_marker.controls.append(rotate_control)
# add the interactive marker to our collection &
# tell the server to call processFeedback() when feedback arrives for it
control = InteractiveMarkerControl()
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
server.insert(int_marker, self.processFeedback)
# 'commit' changes and send to all clients
server.applyChanges()
def processFeedback(self, feedback):
p = feedback.pose.position
self.tf_listener.waitForTransform("/base_link", "/world",
rospy.Time(0), rospy.Duration(1))
pp = PointStamped()
pp.point = p
pp.header.frame_id = "/world"
point_after = self.tf_listener.transformPoint("arm_base_link", pp)
x = point_after.point.x #+ random.uniform(0.005, -0.005)
y = point_after.point.y #+ random.uniform(0.005, -0.005)
# print "x: ",x, " y: ",y
z = point_after.point.z
ik_candidate = ik_solver(x, y, z, -90)
# print ik_candidate
if not np.isnan(ik_candidate.all()):
for theta_1, theta_2, theta_3, theta_4 in ik_candidate:
if abs(theta_2) < 2 and abs(theta_3) < 2 and abs(theta_4) < 2:
self.ik = [theta_1, theta_2, theta_3, theta_4]
break
# print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z)
def home_pose(self):
joint_values = self.group.get_current_joint_values()
joint_values[0] = 0
joint_values[1] = -1.2417246372984723
joint_values[2] = 1.7997449697893113
joint_values[3] = 1.4184705050066115
self.group.set_joint_value_target(joint_values)
plan2 = self.group.plan()
self.group.execute(plan2, wait=True)
def start_ik(self, theta_1, theta_2, theta_3, theta_4):
joint_values = self.group.get_current_joint_values()
joint_values[0] = theta_1
joint_values[1] = theta_2
joint_values[2] = theta_3
joint_values[3] = theta_4
self.group.set_joint_value_target(joint_values)
plan2 = self.group.plan()
return plan2
# self.group.execute(plan2,wait=True)
def planCB(self, point_before, switch):
# print msg
self.tf_listener.waitForTransform("/base_link", "/world",
rospy.Time(0), rospy.Duration(1))
point_after = self.tf_listener.transformPoint("base_link",
point_before)
x = point_after.point.x #+ random.uniform(0.005, -0.005)
y = point_after.point.y #+ random.uniform(0.005, -0.005)
# print "x: ",x, " y: ",y
z = point_after.point.z
# print "x: ",x, " y: ",y," z: ",z
pose_target = Pose()
pose_target.position.x = x
pose_target.position.y = y
pose_target.position.z = z
r = math.atan(y / x)
p = math.radians(90)
y = 0
if switch:
self.rotation(r)
# while (1):
# q = tf.transformations.quaternion_from_euler(math.radians(msg.data[3]), math.radians(msg.data[4]), math.radians(msg.data[5]))
q = tf.transformations.quaternion_from_euler(r, p, y)
pose_target.orientation.x = q[0]
pose_target.orientation.y = q[1]
pose_target.orientation.z = q[2]
pose_target.orientation.w = q[3]
# print pose_target
# self.group.set_goal_tolerance(0.1)
self.group.set_pose_target(pose_target)
# self.group.set_position_target((msg.data[0],msg.data[1],msg.data[2]))
self.plan = self.group.plan()
# print self.group.plan()
ok = self.group.execute(self.plan, wait=True)
print ok
return ok
# if ok:
# break
# # print self.plan
# p += 0.01
# print p
def onShutdown(self):
rospy.loginfo("[%s] Shutting down..." % self.node_name)
rospy.sleep(0.5) #To make sure that it gets published.
rospy.loginfo("[%s] Shutdown" % self.node_name)
class WaypointWindow(QWidget):
def __init__(self, active_aircrafts):
super(WaypointWindow, self).__init__()
self.setWindowTitle("Ground Control Station")
self.adjustSize()
self.move(200, 100)
self.waypoint_plaintext_dict = {}
self.main_layout = QHBoxLayout()
self.progressbar_layout = QVBoxLayout()
self.progressbar_layout.setAlignment(Qt.AlignTop)
# Why we separate this is so that we can refresh the layout
self.create_layout(active_aircrafts)
self.main_layout.addLayout(self.progressbar_layout)
self.setLayout(self.main_layout)
def create_layout(self, active_aircrafts):
for i in active_aircrafts:
self.create_progressbar(i)
def create_progressbar(self, aircraft_no):
waypoint_layout = QVBoxLayout()
waypoint_layout.setContentsMargins(0, 10, 0, 10)
waypoint_header_layout = QHBoxLayout(
) # waypoint_header_layout will be nested inside waypoint_layout with the progress bar beneath it
aircraft_label = QLabel('Aircraft ' + str(aircraft_no))
self.waypoint_plaintext_dict['aircraft' +
str(aircraft_no)] = QPlainTextEdit()
self.waypoint_plaintext_dict.get('aircraft' +
str(aircraft_no)).setMaximumHeight(40)
self.waypoint_plaintext_dict.get('aircraft' +
str(aircraft_no)).setReadOnly(True)
self.waypoint_plaintext_dict[
'aircraftlink' + str(aircraft_no)] = QPlainTextEdit("Telegram")
self.waypoint_plaintext_dict.get('aircraftlink' +
str(aircraft_no)).setMaximumHeight(40)
self.waypoint_plaintext_dict.get('aircraftlink' +
str(aircraft_no)).setMaximumWidth(100)
self.waypoint_plaintext_dict.get('aircraftlink' +
str(aircraft_no)).setReadOnly(True)
self.waypoint_plaintext_dict['progress_bar_aircraft' +
str(aircraft_no)] = QProgressBar()
waypoint_header_layout.addWidget(aircraft_label)
waypoint_header_layout.addWidget(
self.waypoint_plaintext_dict['aircraft' + str(aircraft_no)])
waypoint_header_layout.addWidget(
self.waypoint_plaintext_dict['aircraftlink' + str(aircraft_no)])
waypoint_layout.addLayout(waypoint_header_layout)
waypoint_layout.addWidget(
self.waypoint_plaintext_dict['progress_bar_aircraft' +
str(aircraft_no)])
self.progressbar_layout.addLayout(waypoint_layout)
def remove(self, layout):
for i in reversed(range(layout.count())):
child = layout.takeAt(0)
if child.widget():
child.widget().deleteLater()
if child.layout():
self.remove(child.layout())
def shutdown(self):
self.close()
class NavViewWidget(QWidget):
def __init__(self, map_topic='/map',
paths=['/move_base/NavFn/plan', '/move_base/TrajectoryPlannerROS/local_plan'],
polygons=['/move_base/local_costmap/robot_footprint']):
super(NavViewWidget, self).__init__()
self._layout = QVBoxLayout()
self._button_layout = QHBoxLayout()
self.setAcceptDrops(True)
self.setWindowTitle('Navigation Viewer')
self.paths = paths
self.polygons = polygons
self.map = map_topic
self._tf = tf.TransformListener()
self._nav_view = NavView(map_topic, paths, polygons, tf = self._tf, parent = self)
self._set_pose = QPushButton('Set Pose')
self._set_pose.clicked.connect(self._nav_view.pose_mode)
self._set_goal = QPushButton('Set Goal')
self._set_goal.clicked.connect(self._nav_view.goal_mode)
self._button_layout.addWidget(self._set_pose)
self._button_layout.addWidget(self._set_goal)
self._layout.addLayout(self._button_layout)
self._layout.addWidget(self._nav_view)
self.setLayout(self._layout)
def dragEnterEvent(self, e):
if not e.mimeData().hasText():
if not hasattr(e.source(), 'selectedItems') or len(e.source().selectedItems()) == 0:
qWarning('NavView.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
return
item = e.source().selectedItems()[0]
topic_name = item.data(0, Qt.UserRole)
if topic_name == None:
qWarning('NavView.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
return
else:
topic_name = str(e.mimeData().text())
if accepted_topic(topic_name):
e.accept()
e.acceptProposedAction()
def dropEvent(self, e):
if e.mimeData().hasText():
topic_name = str(e.mimeData().text())
else:
droped_item = e.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
topic_type, array = get_field_type(topic_name)
if not array:
if topic_type is OccupancyGrid:
self.map = topic_name
# Swap out the nav view for one with the new topics
self._nav_view.close()
self._nav_view = NavView(self.map, self.paths, self.polygons, self._tf, self)
self._layout.addWidget(self._nav_view)
elif topic_type is Path:
self.paths.append(topic_name)
self._nav_view.add_path(topic_name)
elif topic_type is PolygonStamped:
self.polygons.append(topic_name)
self._nav_view.add_polygon(topic_name)
def save_settings(self, plugin_settings, instance_settings):
self._nav_view.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self._nav_view.restore_settings(plugin_settings, instance_settings)
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
class BagParser(QWidget):
def __init__(self, bag_files, listtopics, duration):
super(BagParser, self).__init__()
# window title
self.setWindowTitle("Making csv file")
# size of window
self.resize(960, 720)
#self.showFullScreen()
#self.setWindowState(Qt.WindowMaximized)
# print listtopics
# print E.get_general_features_options()
# print E.get_specific_features_options()
self.topics_items = dict()
self.topics_items["0"] = listtopics
self.topics_items["1"] = E.get_general_features_options()
self.topics_items["2"] = E.get_specific_features_options()
print self.topics_items
#path to bag file
self.bag_files = bag_files
self.selected_bag_topics = []
self.selected_specific_features = []
self.selected_general_features = []
self.items_list_topics = []
self.area = QScrollArea(self)
self.areagen = QScrollArea(self)
self.areaspec = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.main_widget1 = QWidget(self.areagen)
self.main_widget2 = QWidget(self.areaspec)
self.ok_button = QPushButton("Export To CSV", self)
#self.ok_button.setFixedSize(150, 30)
self.ok_button.clicked.connect(self.onButtonClicked)
self.clear_button = QPushButton("Clear Selection", self)
# self.clear_button.resize(self.clear_button.sizeHint())
self.clear_button.clicked.connect(self.onClearClicked)
self.choose_button = QPushButton("Get Last Export Choose", self)
self.choose_button.clicked.connect(self.onButtonChooseCliked)
self.ok_button.setEnabled(False)
self.label1 = QLabel("Select topic from bag(s)", self)
self.label1.setAlignment(Qt.AlignCenter)
self.label2 = QLabel("Statistics Features", self)
self.label2.setAlignment(Qt.AlignCenter)
self.label3 = QLabel("Specific Features", self)
self.label3.setAlignment(Qt.AlignCenter)
self.duration = duration
self.label5 = QLabel("Duration Time: " + str("%.1f" % duration), self)
self.label5.setAlignment(Qt.AlignCenter)
self.main_vlayout = QVBoxLayout(self)
# self.main_vlayout = QGridLayout(self)
self.main_vlayout.addWidget(self.label1)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.label2)
self.main_vlayout.addWidget(self.areagen)
self.main_vlayout.addWidget(self.label3)
self.main_vlayout.addWidget(self.areaspec)
self.label4 = QLabel("Window time", self)
self.label4.setAlignment(Qt.AlignCenter)
# self.main_vlayout.addWidget(self.label4)
self.window = QLineEdit(self)
# self.main_vlayout.addWidget(self.window)
self.window.setText("1")
self.windows_time_3 = QHBoxLayout(self)
self.windows_time_3.addWidget(self.label4)
self.windows_time_3.addWidget(self.window)
self.windows_time_3.addWidget(self.label5)
self.main_vlayout.addLayout(self.windows_time_3)
# self.window = QLineEdit(self)
# self.window.setText("1")
# self.box = QVBoxLayout()
# self.box.addStretch(1)
# self.box.addWidget(self.clear_button)
# self.box.addWidget(self.choose_button)
# self.box.addWidget(self.label4)
# self.box.addWidget(self.window)
# self.box.addWidget(self.label5)
# self.box.addWidget(self.ok_button)
#self.main_vlayout.addWidget(self.from_nodes_button)
# self.main_vlayout.addLayout(self.box)
self.two_buttons = QHBoxLayout(self)
self.two_buttons.addWidget(self.choose_button)
self.two_buttons.addWidget(self.clear_button)
self.main_vlayout.addLayout(self.two_buttons)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.item_all = MyQCheckBox("All", self, self.selection_vlayout, None)
self.item_all.stateChanged.connect(
lambda x: self.updateList(x, self.item_all, None))
self.selection_vlayout.addWidget(self.item_all)
topic_data_list = listtopics
topic_data_list.sort()
for topic in topic_data_list:
self.addCheckBox(topic, self.selection_vlayout,
self.selected_bag_topics)
self.selection_vlayout1 = QVBoxLayout(self)
self.item_all1 = MyQCheckBox("All", self, self.selection_vlayout1,
None)
self.item_all1.stateChanged.connect(
lambda x: self.updateList(x, self.item_all1, None))
self.selection_vlayout1.addWidget(self.item_all1)
topic_data_list1 = E.get_general_features_options()
topic_data_list1.sort()
for topic in topic_data_list1:
self.addCheckBox(topic, self.selection_vlayout1,
self.selected_general_features)
self.selection_vlayout2 = QVBoxLayout(self)
self.item_all2 = MyQCheckBox("All", self, self.selection_vlayout2,
None)
self.item_all2.stateChanged.connect(
lambda x: self.updateList(x, self.item_all2, None))
self.selection_vlayout2.addWidget(self.item_all2)
topic_data_list2 = E.get_specific_features_options()
topic_data_list2.sort()
for topic in topic_data_list2:
self.addCheckBox(topic, self.selection_vlayout2,
self.selected_specific_features)
self.main_widget.setLayout(self.selection_vlayout)
self.main_widget1.setLayout(self.selection_vlayout1)
self.main_widget2.setLayout(self.selection_vlayout2)
self.area.setWidget(self.main_widget)
self.areagen.setWidget(self.main_widget1)
self.areaspec.setWidget(self.main_widget2)
self.show()
def onClearClicked(self):
self.clearTopicCheckState()
def clearTopicCheckState(self):
for item in self.items_list_topics:
item.setCheckState(False)
self.item_all.setCheckState(False)
self.item_all1.setCheckState(False)
self.item_all2.setCheckState(False)
def addCheckBox(self, topic, selection_vlayout, selected_list):
item = MyQCheckBox(topic, self, selection_vlayout, selected_list)
item.stateChanged.connect(lambda x: self.updateList(x, item, topic))
self.items_list_topics.append(item)
selection_vlayout.addWidget(item)
def changeTopicCheckState(self, topic, state):
for item in self.items_list_topics:
if item.text() == topic:
item.setCheckState(state)
return
def updateList(self,
state,
item_clicked,
topic=None,
force_update_state=False):
if topic is None: # The "All" checkbox was checked / unchecked
# print "if topic is None"
if state == Qt.Checked:
self.item_all.setTristate(False)
for item in self.items_list_topics:
if item.checkState() == Qt.Unchecked and \
item.selection_vlayout == item_clicked.selection_vlayout:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
self.item_all.setTristate(False)
for item in self.items_list_topics:
if item.checkState() == Qt.Checked and \
item.selection_vlayout == item_clicked.selection_vlayout:
item.setCheckState(Qt.Unchecked)
else:
# print "else:"
if state == Qt.Checked:
item_clicked.selected_list.append(topic)
print item_clicked.selected_list
else:
item_clicked.selected_list.remove(topic)
#if self.item_all.checkState() == Qt.Checked:
# self.item_all.setCheckState(Qt.PartiallyChecked)
if self.selected_specific_features != []:
if self.selected_general_features != [] and self.selected_bag_topics != []:
self.ok_button.setEnabled(True)
elif self.selected_general_features == [] and self.selected_bag_topics == []:
self.ok_button.setEnabled(True)
else:
self.ok_button.setEnabled(False)
else:
if self.selected_general_features != [] and self.selected_bag_topics != []:
self.ok_button.setEnabled(True)
else:
self.ok_button.setEnabled(False)
# if self.selected_bag_topics != []:
# if self.selected_specific_features == [] and self.selected_general_features == []:
# self.ok_button.setEnabled(False)
# else:
# self.ok_button.setEnabled(True)
# # elif self.selected_specific_features != [] and self.selected_general_features == []:
# # self.ok_button.setEnabled(True)
# # elif self.selected_specific_features == [] and self.selected_general_features != []:
# # self.ok_button.setEnabled(True)
# else:
# self.ok_button.setEnabled(False)
# if self.selected_specific_features != []:
# if self.selected_bag_topics == [] and self.selected_general_features == []:
# self.ok_button.setEnabled(True)
# elif self.selected_bag_topics != [] and self.selected_general_features != []:
# self.ok_button.setEnabled(True)
# else:
# self.ok_button.setEnabled(False)
# else:
# if self.selected_bag_topics == [] or self.selected_general_features == []:
# self.ok_button.setEnabled(False)
# else:
# self.ok_button.setEnabled(True)
def onButtonChooseCliked(self):
for checkbox in self.items_list_topics:
checkbox.setCheckState(Qt.Unchecked)
with open(get_path() + "logger.log", 'r') as f:
topics = f.read().splitlines()
for checkbox in self.items_list_topics:
if checkbox.text() in topics:
checkbox.setCheckState(Qt.Checked)
def get_current_opened_directory(self, filepath):
import os
direc = "/"
if os.path.isfile(filepath):
with open(filepath, 'r') as f:
direc = f.read()
return direc
def onButtonClicked(self):
import inspect, os
filepath = os.path.dirname(
os.path.abspath(inspect.getfile(
inspect.currentframe()))) + "/log/save_csv.log"
current_directory = self.get_current_opened_directory(filepath)
window = self.window.text()
try:
val = float(window)
except ValueError:
QMessageBox.about(self, "Error in Window Time",
"That's not a number!")
return
if val >= self.duration:
QMessageBox.about(
self, "Error in Window Time",
"time need to be smaller than: " + str(self.duration))
return
# filename = QFileDialog.getSaveFileName(self, self.tr('csv File'), current_directory, self.tr('csv (*.csv)'))
saved_dir = str(
QFileDialog.getExistingDirectory(self, "Select Directory",
current_directory))
# if filename[0] != '':
# with open(filepath, "w") as f:
# f.write(filename[0])
if saved_dir != '':
with open(filepath, "w") as f:
f.write(saved_dir)
topics = self.selected_bag_topics
specific_features_selection = self.selected_specific_features
general_features_selection = self.selected_general_features
with open(get_path() + 'logger.log', "w") as f:
for topic in topics:
f.write(topic + "\n")
for topic1 in specific_features_selection:
f.write(topic1 + "\n")
for topic2 in general_features_selection:
f.write(topic2 + "\n")
ef = E.ExtractFeatures(topics, float(window),
specific_features_selection,
general_features_selection)
counter = 0
for bag_file in self.bag_files:
df = ef.generate_features(bag_file)
if len(self.bag_files) == 1:
counter = -1
# temp = filename + "/" +
# temp = get_corrent_file_name(filename[0], ".csv", counter)
csv_path = generate_csv_from_bag(saved_dir, bag_file)
# temp = "%s_%s%s" % (filename[0],counter,".csv")
E.write_to_csv(csv_path, df)
counter = counter + 1
QMessageBox.about(self, "csv export",
"csv was exported successfuly")
class TopicSelection(QWidget):
recordSettingsSelected = Signal(bool, list, dict)
def __init__(self):
super(TopicSelection, self).__init__()
master = rosgraph.Master('rqt_bag_recorder')
self.setWindowTitle("Record a Simulation")
self.resize(650, 720)
pre = TS.get_time_series_pre_feature_options()
glob = TS.get_global_time_series_features_options()
# print pre
# print glob
all_topics = S.get_topics_options()
keys = all_topics.keys()
# print all_topics.keys()[0]
self.plp_filename = ""
self.group_selected_items = dict()
self.group_areas = dict()
self.group_main_widget = dict()
self.group_selection_vlayout = dict()
self.group_item_all = dict()
# self.main_vlayout = QVBoxLayout(self)
self.main_vlayout = QVBoxLayout(self)
self.group_label = dict()
self.selected_topics = []
self.items_list = []
for group_name in all_topics:
self.group_selected_items[group_name] = []
self.group_areas[group_name] = QScrollArea(self)
self.group_main_widget[group_name] = QWidget(
self.group_areas[group_name])
self.group_label[group_name] = QLabel(group_name, self)
self.group_label[group_name].setAlignment(Qt.AlignCenter)
self.main_vlayout.addWidget(self.group_label[group_name])
self.main_vlayout.addWidget(self.group_areas[group_name])
self.group_selection_vlayout[group_name] = QVBoxLayout(self)
self.group_item_all[group_name] = MyQCheckBox(
"All", self, self.group_selection_vlayout[group_name], None)
self.MakeCheckBoxList(self.group_selection_vlayout[group_name],
self.group_selected_items[group_name],
all_topics[group_name],
self.group_item_all[group_name])
self.group_main_widget[group_name].setLayout(
self.group_selection_vlayout[group_name])
self.group_areas[group_name].setWidget(
self.group_main_widget[group_name])
self.label1 = QLabel("Scenarios", self)
self.label1.setAlignment(Qt.AlignCenter)
self.main_vlayout.addWidget(self.label1)
scanarios = S.get_scenarios_options()
self.scanarios_answer = scanarios
self.map_answer = dict()
# print scanarios
keys1 = scanarios.keys()
# print keys1[0]
# print scanarios[keys1[0]]["name"]
# print scanarios[keys1[0]]["params"]
#
# for item in scanarios[keys1[0]]["params"]:
# print item
self.radio_items = dict()
self.number_group = QButtonGroup(self)
for id_radio in scanarios:
self.radio_items[id_radio] = QRadioButton(
scanarios[id_radio]["name"])
self.number_group.addButton(self.radio_items[id_radio])
self.main_vlayout.addWidget(self.radio_items[id_radio])
self.radio_items[id_radio].setChecked(False)
self.radio_items[id_radio].clicked.connect(
partial(self.callConsult, scanarios[id_radio]["params"],
id_radio))
self.select_path = QLineEdit()
self.save_path = QLineEdit()
self.select_path.setEnabled(False)
self.save_path.setEnabled(False)
self.plp_button = QPushButton("Select PLP Python File...", self)
self.plp_button.clicked.connect(self.onPlpClicked)
self.two_buttons1 = QHBoxLayout(self)
self.two_buttons1.addWidget(self.plp_button)
self.two_buttons1.addWidget(self.select_path)
self.main_vlayout.addLayout(self.two_buttons1)
# self.label = QLabel("live Topics", self)
# self.label.setAlignment(Qt.AlignCenter)
#
# self.main_vlayout.addWidget(self.label)
# self.area = QScrollArea(self)
# self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Record", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.choose_button = QPushButton("Get Last Export Choose", self)
self.choose_button.clicked.connect(self.onButtonChooseCliked)
self.clear_button = QPushButton("Clear Selection", self)
self.clear_button.clicked.connect(self.onClearClicked)
self.choose_clear_buttons = QHBoxLayout(self)
self.choose_clear_buttons.addWidget(self.choose_button)
self.choose_clear_buttons.addWidget(self.clear_button)
self.main_vlayout.addLayout(self.choose_clear_buttons)
# self.main_vlayout.addRow(self.choose_button, self.clear_button)
# self.from_nodes_button = QPushButton("From Nodes", self)
# self.from_nodes_button.clicked.connect(self.onFromNodesButtonClicked)
# self.main_vlayout.addWidget(self.area)
# self.main_vlayout.addWidget(self.choose_button)
self.main_vlayout.addWidget(self.ok_button)
# self.main_vlayout.addWidget(self.from_nodes_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
# self.item_all = MyQCheckBox("All", self, self.selection_vlayout, None)
# self.item_all.stateChanged.connect(lambda x: self.updateList(x, self.item_all, None))
# self.selection_vlayout.addWidget(self.item_all)
# topic_data_list4 = map(lambda l: l[0], master.getPublishedTopics(''))
# topic_data_list4.sort()
# for topic in topic_data_list4:
# self.addCheckBox(topic, self.selection_vlayout, self.selected_topics)
# self.main_widget.setLayout(self.selection_vlayout)
# self.area.setWidget(self.main_widget)
# print S.get_scenarios_options()
self.show()
def onClearClicked(self):
self.clearTopicCheckState()
def clearTopicCheckState(self):
for item in self.items_list:
item.setCheckState(False)
for item in self.group_item_all.values():
item.setCheckState(False)
def onButtonChooseCliked(self):
for checkbox in self.items_list:
checkbox.setCheckState(Qt.Unchecked)
with open(get_path() + "logger_topic.log", 'r') as f:
topics = f.read().splitlines()
for checkbox in self.items_list:
if checkbox.text() in topics:
checkbox.setCheckState(Qt.Checked)
def callConsult(self, params, id_radio):
self.input_dialog = inputDialog(params, id_radio)
self.input_dialog.ParamsSelected.connect(self.params_answer)
# item, ok = QInputDialog.getItem(self, "select parameter",
# "list of parameters", params, 0, False)
# if ok and item:
# self.gettext(item)
def params_answer(self, params, label_items, id_radio):
if id_radio == 0:
# print "------" + str(id_radio)
self.number_group.setExclusive(False)
# print self.radio_items
for item in self.radio_items:
# print self.radio_items[item]
self.radio_items[item].setChecked(False)
self.number_group.setExclusive(True)
self.enable_record()
else:
# print id_radio
# print params
# print params.values()
a = {}
# print label_items
for item, name in zip(params, label_items):
# value = params[item].text()
a[name] = item.text().encode("utf-8")
# print a
self.map_answer = {"id": id_radio, "params": a}
self.enable_record()
return self.map_answer
# for item in params.values():
# print item.text()
# pass
# def gettext(self, item):
# text, ok = QInputDialog.getText(self, 'Text Input Dialog', item)
#
# if ok:
# print self.multipleReplace(str(text), item)
#
#
# def multipleReplace(self, text, item):
# for key in self.scanarios_answer:
# #
# print self.scanarios_answer[key]["params"]
# for id, par in enumerate(self.scanarios_answer[key]["params"]):
# if par == item:
# self.scanarios_answer[key]["params"][id] = text
# for id_radio in self.radio_items.keys():
# if self.radio_items[id_radio].isChecked():
# self.map_answer = {"id" : id_radio, "params" : self.scanarios_answer[id_radio]["params"]}
# return self.map_answer
def MakeCheckBoxList(self, selection_vlayout, selected, topics_Keys,
item_all):
item_all.stateChanged.connect(
lambda x: self.updateList(x, item_all, None))
selection_vlayout.addWidget(item_all)
topic_data_list = topics_Keys
topic_data_list.sort()
for topic in topic_data_list:
self.addCheckBox(topic, selection_vlayout, selected)
def get_item_by_name(self, item_name):
for item in self.items_linputDialogist:
if item.text() == item_name:
return item
return None
def addCheckBox(self, topic, selection_vlayout, selected_list):
item = MyQCheckBox(topic, self, selection_vlayout, selected_list)
item.stateChanged.connect(lambda x: self.updateList(x, item, topic))
self.items_list.append(item)
selection_vlayout.addWidget(item)
def changeTopicCheckState(self, topic, state):
for item in self.items_list:
if item.text() == topic:
item.setCheckState(state)
return
# def updateList(self, state, topic=None, force_update_state=False):
# if topic is None: # The "All" checkbox was checked / unchecked
# if state == Qt.Checked:
# self.item_all.setTristate(False)
# for item in self.items_list:
# if item.checkState() == Qt.Unchecked:
# item.setCheckState(Qt.Checked)
# elif state == Qt.Unchecked:
# self.item_all.setTristate(False)
# for item in self.items_list:
# if item.checkState() == Qt.Checked:
# item.setCheckState(Qt.Unchecked)
# else:
# if state == Qt.Checked:
# self.selected_topics.append(topic)
# else:
# self.selected_topics.remove(topic)
# if self.item_all.checkState() == Qt.Checked:
# self.item_all.setCheckState(Qt.PartiallyChecked)
#
# if self.selected_topics == []:
# self.ok_button.setEnabled(False)
# else:
# self.ok_button.setEnabled(True)
def updateList(self,
state,
item_clicked,
topic=None,
force_update_state=False):
if type(item_clicked) is str:
item_clicked = self.get_item_by_name(item_clicked)
if item_clicked is None:
return
if topic is None: # The "All" checkbox was checked / unchecked
#print "if topic is None"
if state == Qt.Checked:
# self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Unchecked and \
item.selection_vlayout == item_clicked.selection_vlayout:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
# self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Checked and \
item.selection_vlayout == item_clicked.selection_vlayout:
item.setCheckState(Qt.Unchecked)
else:
if state == Qt.Checked:
item_clicked.selected_list.append(topic)
#print item_clicked.selected_list
else:
item_clicked.selected_list.remove(topic)
#if self.item_all.checkState() == Qt.Checked:
# self.item_all.setCheckState(Qt.PartiallyChecked)
self.enable_record()
def enable_record(self):
temp_selected_topics = []
for item in self.group_selected_items.values():
if item:
for i in item:
temp_selected_topics.append(i)
flag = False
for item in self.radio_items:
if self.radio_items[item].isChecked():
flag = True
break
# print self.radio_items[item]
# flag = reduce(lambda acc, curr: acc or , self.radio_items, False)
# print "------+++++-" + str(flag)
if len(temp_selected_topics) > 0 or len(self.selected_topics) > 0:
self.ok_button.setEnabled(flag)
else:
self.ok_button.setEnabled(False)
def onButtonClicked(self):
# print self.group_selected_items
for item in self.group_selected_items.values():
if item:
for i in item:
self.selected_topics.append(i)
topics = self.selected_topics
with open(get_path() + 'logger_topic.log', "w") as f:
for topic in topics:
f.write(topic + "\n")
self.close()
# TBD - runing plp on shell on itself
# if self.plp_filename != "":
# from .plp import Plp
# Plp(self.plp_filename)
self.recordSettingsSelected.emit(False, self.selected_topics,
self.map_answer)
def get_current_opened_directory(self, filepath):
import os
direc = "/"
if os.path.isfile(filepath):
with open(filepath, 'r') as f:
pathes = f.read()
direc = pathes.rsplit('/', 1)[0]
return direc
def onPlpClicked(self):
import inspect, os
filepath = os.path.dirname(
os.path.abspath(inspect.getfile(
inspect.currentframe()))) + "/log/save_plp.log"
current_directory = self.get_current_opened_directory(filepath)
fd = QFileDialog(self)
wc = "Csv files {.py} (*.py)"
# print current_directory
filename, filter = fd.getOpenFileNamesAndFilter(
filter=wc, initialFilter=('*.py'), directory=current_directory)
if len(filename):
self.plp_filename = filename[0]
with open(filepath, "w") as f:
f.write(self.plp_filename)
self.select_path.setText(self.plp_filename)
# print self.plp_filename[0]
def onFromNodesButtonClicked(self):
self.node_selection = NodeSelection(self)
def getTopicsByName(self, name):
arr = S.get_topics_options()
return arr[name]
class QuestionDialogPlugin(Plugin):
timeout_sig = pyqtSignal()
update_sig = pyqtSignal()
def __init__(self, context):
super(QuestionDialogPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('QuestionDialogPlugin')
font_size = rospy.get_param("~font_size", 40)
# Create QWidget
self._widget = QWidget()
self._widget.setFont(QFont("Times", font_size, QFont.Bold))
self._layout = QVBoxLayout(self._widget)
self._text_browser = QTextBrowser(self._widget)
self._layout.addWidget(self._text_browser)
self._button_layout = QGridLayout()
self._layout.addLayout(self._button_layout)
# layout = QVBoxLayout(self._widget)
# layout.addWidget(self.button)
self._widget.setObjectName('QuestionDialogPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Setup service provider
self.service = rospy.Service('question_dialog', QuestionDialog,
self.service_callback)
self.request = None
self.response_ready = False
self.response = None
self.buttons = []
self.text_label = None
self.text_input = None
self.update_sig.connect(self.update)
self.timeout_sig.connect(self.timeout)
def shutdown_plugin(self):
self.service.shutdown()
def service_callback(self, req):
self.response_ready = False
self.request = req
self.update_sig.emit()
# Start timer against wall clock here instead of the ros clock.
start_time = time.time()
while not self.response_ready:
if self.request != req:
# The request got preempted by a new request.
return QuestionDialogResponse(QuestionDialogRequest.PREEMPTED, "")
if req.timeout != QuestionDialogRequest.NO_TIMEOUT:
current_time = time.time()
if current_time - start_time > req.timeout:
self.timeout_sig.emit()
return QuestionDialogResponse(
QuestionDialogRequest.TIMED_OUT, "")
time.sleep(0.2)
return self.response
def update(self):
self.clean()
if not self.request:
rospy.logwarn('question dialog: no request, update ignored')
return
req = self.request
self._text_browser.setText(req.message)
if req.type == QuestionDialogRequest.DISPLAY:
# All done, nothing more too see here.
self.response = QuestionDialogResponse(
QuestionDialogRequest.NO_RESPONSE, "")
self.response_ready = True
elif req.type == QuestionDialogRequest.CHOICE_QUESTION:
for index, options in enumerate(req.options):
button = QPushButton(options, self._widget)
button.clicked.connect(partial(self.handle_button, index))
row = index / 3
col = index % 3
self._button_layout.addWidget(button, row, col)
self.buttons.append(button)
elif req.type == QuestionDialogRequest.TEXT_QUESTION:
self.text_label = QLabel("Enter here: ", self._widget)
self._button_layout.addWidget(self.text_label, 0, 0)
self.text_input = QLineEdit(self._widget)
self.text_input.editingFinished.connect(self.handle_text)
self._button_layout.addWidget(self.text_input, 0, 1)
def timeout(self):
self._text_browser.setText("Oh no! The request timed out.")
self.clean()
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
self.buttons = []
self.text_input = None
self.text_label = None
def handle_button(self, index):
self.response = QuestionDialogResponse(index, "")
self.clean()
self.response_ready = True
def handle_text(self):
self.response = QuestionDialogResponse(
QuestionDialogRequest.TEXT_RESPONSE,
self.text_input.text())
self.clean()
self.response_ready = True
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 HistorySelection(QWidget):
def __init__(self):
super(HistorySelection, self).__init__()
self.setWindowTitle("Extracting Window Features")
self.resize(550, 440)
per_title, pre = TS.get_time_series_pre_feature_options()
glob_title, glob = TS.get_global_time_series_features_options()
self.history_items = dict()
self.history_items[per_title] = pre
self.history_items[glob_title] = glob
self.saved_dir = []
self.group_selected_items = dict()
self.group_areas = dict()
self.group_main_widget = dict()
self.group_selection_vlayout = dict()
self.group_item_all = dict()
self.main_vlayout = QVBoxLayout(self)
self.group_label = dict()
self.items_list = []
self.selected_topics = []
self.files = []
# layout = QFormLayout()
self.select_path = QLineEdit()
self.save_path = QLineEdit()
self.select_path.setEnabled(False)
self.save_path.setEnabled(False)
self.ok_button = QPushButton("Select CSV...", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.two_buttons1 = QHBoxLayout(self)
self.two_buttons1.addWidget(self.ok_button)
self.two_buttons1.addWidget(self.select_path)
self.main_vlayout.addLayout(self.two_buttons1)
# self.main_vlayout.addRow(self.ok_button, self.client_answers)
# self.main_vlayout.addWidget(self.ok_button)
# self.main_vlayout.addWidget(self.ok_button)
for group_name in self.history_items:
self.group_selected_items[group_name] = []
self.group_areas[group_name] = QScrollArea(self)
self.group_main_widget[group_name] = QWidget(
self.group_areas[group_name])
self.group_label[group_name] = QLabel(group_name, self)
self.group_label[group_name].setAlignment(Qt.AlignCenter)
self.main_vlayout.addWidget(self.group_label[group_name])
self.main_vlayout.addWidget(self.group_areas[group_name])
self.group_selection_vlayout[group_name] = QVBoxLayout(self)
self.group_item_all[group_name] = MyQCheckBox(
"All", self, self.group_selection_vlayout[group_name], None)
self.MakeCheckBoxList(self.group_selection_vlayout[group_name],
self.group_selected_items[group_name],
self.history_items[group_name],
self.group_item_all[group_name])
self.group_main_widget[group_name].setLayout(
self.group_selection_vlayout[group_name])
self.group_areas[group_name].setWidget(
self.group_main_widget[group_name])
self.clear_button = QPushButton("Clear Selection", self)
self.clear_button.clicked.connect(self.onClearClicked)
self.choose_button = QPushButton("Get Last Export Choose", self)
self.choose_button.clicked.connect(self.onButtonChooseCliked)
# self.main_vlayout.addWidget(self.choose_button)
self.label4 = QLabel("Window time", self)
self.label4.setAlignment(Qt.AlignCenter)
# self.main_vlayout.addWidget(self.label4)
self.window = QLineEdit(self)
# self.main_vlayout.addWidget(self.window)
self.window.setText("3")
self.label5 = QLabel("Duration Time:", self)
self.label5.setAlignment(Qt.AlignCenter)
self.label6 = QLabel("Step:", self)
self.label6.setAlignment(Qt.AlignCenter)
self.step = QLineEdit(self)
self.step.setText("1")
self.windows_time_3 = QHBoxLayout(self)
self.windows_time_3.addWidget(self.label4)
self.windows_time_3.addWidget(self.window)
self.windows_time_3.addWidget(self.label5)
self.main_vlayout.addLayout(self.windows_time_3)
self.step_2 = QHBoxLayout(self)
self.step_2.addWidget(self.label6)
self.step_2.addWidget(self.step)
self.main_vlayout.addLayout(self.step_2)
# self.main_vlayout.addRow(self.label4, self.window)
self.two_buttons = QHBoxLayout(self)
self.two_buttons.addWidget(self.choose_button)
self.two_buttons.addWidget(self.clear_button)
self.main_vlayout.addLayout(self.two_buttons)
# self.main_vlayout.addRow(self.clear_button, self.choose_button)
# self.label5 = QLabel("Load CSV", self)
# self.label5.setAlignment(Qt.AlignCenter)
#
# self.main_vlayout.addWidget(self.label5)
# self.label6 = QLabel("output", self)
# self.label6.setAlignment(Qt.AlignCenter)
#
# self.main_vlayout.addWidget(self.label6)
self.save_button = QPushButton("Save CSV...", self)
self.save_button.clicked.connect(self.onSaveClicked)
self.submit_button = QPushButton("Sumbit", self)
self.submit_button.clicked.connect(self.onSumbitClicked)
# self.main_vlayout.addWidget(self.save_button)
self.two_buttons2 = QHBoxLayout(self)
self.two_buttons2.addWidget(self.save_button)
self.two_buttons2.addWidget(self.save_path)
self.main_vlayout.addLayout(self.two_buttons2)
# self.main_vlayout.addRow(self.save_button, self.client_answers1)
self.main_vlayout.addWidget(self.submit_button)
self.show()
def get_min_rows_csv(self, csv_files):
import pandas as pd
dfs = map(lambda x: pd.read_csv(x, header=0), csv_files)
number = dfs[0].shape[0]
number = reduce(lambda acc, curr: min(acc, curr.shape[0]), dfs, number)
self.label5.setText("Duration Time: %s" % number)
def onClearClicked(self):
self.clearTopicCheckState()
def clearTopicCheckState(self):
for item in self.items_list:
item.setCheckState(False)
for item in self.group_item_all.values():
item.setCheckState(False)
def MakeCheckBoxList(self, selection_vlayout, selected, topics_Keys,
item_all):
item_all.stateChanged.connect(
lambda x: self.updateList(x, item_all, None))
selection_vlayout.addWidget(item_all)
topic_data_list = topics_Keys
topic_data_list.sort()
for topic in topic_data_list:
self.addCheckBox(topic, selection_vlayout, selected)
def addCheckBox(self, topic, selection_vlayout, selected_list):
item = MyQCheckBox(topic, self, selection_vlayout, selected_list)
item.stateChanged.connect(lambda x: self.updateList(x, item, topic))
self.items_list.append(item)
selection_vlayout.addWidget(item)
def updateList(self,
state,
item_clicked,
topic=None,
force_update_state=False):
if type(item_clicked) is str:
item_clicked = self.get_item_by_name(item_clicked)
if item_clicked is None:
return
if topic is None: # The "All" checkbox was checked / unchecked
# print "if topic is None"
if state == Qt.Checked:
# self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Unchecked and \
item.selection_vlayout == item_clicked.selection_vlayout:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
# self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Checked and \
item.selection_vlayout == item_clicked.selection_vlayout:
item.setCheckState(Qt.Unchecked)
else:
if state == Qt.Checked:
item_clicked.selected_list.append(topic)
# print item_clicked.selected_list
else:
item_clicked.selected_list.remove(topic)
# if self.item_all.checkState() == Qt.Checked:
# self.item_all.setCheckState(Qt.PartiallyChecked)
def get_current_opened_directory(self, filepath):
import os
direc = "/"
if os.path.isfile(filepath):
with open(filepath, 'r') as f:
direc = f.read()
return direc
def onButtonClicked(self):
import inspect, os
filepath = os.path.dirname(
os.path.abspath(inspect.getfile(
inspect.currentframe()))) + "/log/select_history.log"
fd = QFileDialog(self)
wc = "Csv files {.csv} (*.csv)"
self.files = []
current_directory = self.get_current_opened_directory(filepath)
tmp_pathes, filter = fd.getOpenFileNamesAndFilter(
filter=wc, initialFilter=('*.csv'), directory=current_directory)
for path in tmp_pathes:
self.files.append(path.encode("utf-8"))
# print self.files
if len(self.files) != 0:
self.get_min_rows_csv(tmp_pathes)
self.select_path.setText(self.files[0])
with open(filepath, "w") as f:
f.write(self.files[0])
# handler = logging.FileHandler(filepath, mode='w')
# logger_topic.addHandler(handler)
# logger_topic.info(self.files[0])
else:
self.select_path.setText("")
def check_int(self, number, condition, title, message_body):
try:
# print type(number)
val = int(number)
if val <= condition:
QMessageBox.about(self, title,
"The number should > %s" % condition)
return False
except ValueError:
QMessageBox.about(self, title, message_body)
return False
return True
def check_choose(self):
flag = True
temp = []
for item in self.group_selected_items.values():
if item:
for i in item:
temp.append(i)
if len(temp) == 0:
QMessageBox.about(self, "Features",
"One feature at least should be chosen")
flag = False
return flag
def check_files_amount(self):
flag = True
if len(self.files) <= 0:
QMessageBox.about(self, "Load CSV",
"One file at least should be chosen")
flag = False
return flag
def check_validation(self):
flag = self.check_files_amount()
flag = flag & self.check_choose()
flag = flag & self.check_int(self.window.text(), 2,
"Error in Window Time",
"That's not a number!")
flag = flag & self.check_int(self.step.text(), 0, "Error in Step",
"That's not a integer!")
# TODO selected topic not empty
if self.saved_dir == []:
QMessageBox.about(self, "Save CSV", "Select path for saving")
flag = False
return flag
def onSumbitClicked(self):
if not self.check_validation():
return
self.selected_topics = []
for item in self.group_selected_items.values():
if item:
for i in item:
self.selected_topics.append(i)
# Defined Logging
# handler = logging.FileHandler('/var/tmp/logger_history.log', mode='a')
# logger_topic.addHandler(handler)
topics = self.selected_topics
with open(get_path() + 'logger_history.log', "w") as f:
for topic in topics:
f.write(topic + "\n")
self.createTimeSeriesFeatures(self.files, self.saved_dir,
int(self.window.text()),
self.group_selected_items,
int(self.step.text()))
def onSaveClicked(self):
import inspect, os
filepath = os.path.dirname(
os.path.abspath(inspect.getfile(
inspect.currentframe()))) + "/log/save_history.log"
# print self.files
# print self.window.text()
# print self.group_selected_items.values()
# print self.group_selected_items
current_directory = self.get_current_opened_directory(filepath)
# print current_directory
# self._to_save_filename = QFileDialog.getSaveFileName(self, self.tr('csv File'), current_directory,
# self.tr('csv (*.csv)'))
self.saved_dir = str(
QFileDialog.getExistingDirectory(self, "Select Directory",
current_directory))
# if self._to_save_filename[0] != "":
# with open(filepath, "w") as f:
# f.write(self._to_save_filename[0])
if self.saved_dir != "":
with open(filepath, "w") as f:
f.write(self.saved_dir)
# handler = logging.FileHandler(filepath, mode='w')
# logger_topic.addHandler(handler)
# print self._to_save_filename[0]
# logger_topic.info()
# self.save_path.setText(get_corrent_file_name(self._to_save_filename[0], ".csv"))
self.save_path.setText(self.saved_dir)
def createTimeSeriesFeatures(self, files, saved_dir, window,
group_selected_items, step):
import TimeSeriesFeatures as TS
# saved_dir = saved_dir[0].encode('utf-8')
for input_path in files:
output_path = generate_csv_from_bag(saved_dir, input_path)
print "++++++++++++++++++++++++++ ", output_path
print "in = %s out = %s " % (input_path, output_path)
print "step = %s" % step
ts = TS.TimeSeries(input_path, output_path, window,
group_selected_items, step)
ts.generate_time_series_features()
QMessageBox.about(self, "csv save", "csv was saved successfuly")
def onButtonChooseCliked(self):
for checkbox in self.items_list:
checkbox.setCheckState(Qt.Unchecked)
with open(get_path() + "logger_history.log", 'r') as f:
topics = f.read().splitlines()
for checkbox in self.items_list:
if checkbox.text() in topics:
checkbox.setCheckState(Qt.Checked)
class SliderPublisher(QWidget):
def __init__(self, content):
super(SliderPublisher, self).__init__()
content = content.replace('\t', ' ')
# get message types
self.publishers = {}
self.values = {}
pkgs = []
# to keep track of key ordering in the yaml file
order = []
old = []
for topic, info in yaml.load(content).items():
pkg, msg = info['type'].split('/')
pkgs.append(__import__(pkg, globals(), locals(), ['msg']))
self.publishers[topic] = Publisher(topic,
getattr(pkgs[-1].msg,
msg), info)
order.append((topic, []))
for key in info:
self.values[key] = info[key]
order[-1][1].append((content.find(' ' + key + ':'), key))
old.append((content.find(' ' + key + ':'), key))
for bound in ['min', 'max']:
self.values[key][bound] = float(self.values[key][bound])
self.values[key]['val'] = 0
order[-1][1].sort()
order.sort(key=lambda x: x[1][0][0])
# build sliders - thanks joint_state_publisher
sliderUpdateTrigger = Signal()
self.vlayout = QVBoxLayout(self)
self.gridlayout = QGridLayout()
font = QFont("Helvetica", 9, QFont.Bold)
topic_font = QFont("Helvetica", 10, QFont.Bold)
sliders = []
self.key_map = {}
y = 0
for topic, keys in order:
topic_layout = QVBoxLayout()
label = QLabel(topic)
label.setFont(topic_font)
topic_layout.addWidget(label)
self.gridlayout.addLayout(topic_layout, *(y, 0))
y += 1
for idx, key in keys:
key_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(key)
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)
key_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
key_layout.addWidget(slider)
self.key_map[key] = {
'slidervalue': 0,
'display': display,
'slider': slider
}
slider.valueChanged.connect(self.onValueChanged)
self.gridlayout.addLayout(key_layout, *(y, 0))
y += 1
#sliders.append(key_layout)
# Generate positions in grid and place sliders there
#self.positions = [(y,0) for y in range(len(sliders))]
#for item, pos in zip(sliders, self.positions):
# self.gridlayout.addLayout(item, *pos)
self.vlayout.addLayout(self.gridlayout)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center)
self.vlayout.addWidget(self.ctrbutton)
self.center(1)
def sliderToValue(self, slider, key):
val = self.values[key]
return val['min'] + slider * (val['max'] - val['min']) / RANGE
@pyqtSlot(int)
def onValueChanged(self, event):
# A slider value was changed, but we need to change the joint_info metadata.
for key, key_info in self.key_map.items():
key_info['slidervalue'] = key_info['slider'].value()
# build corresponding value
self.values[key]['val'] = self.sliderToValue(
key_info['slidervalue'], key)
key_info['display'].setText("%.2f" % self.values[key]['val'])
def center(self, event):
for key, key_info in self.key_map.items():
key_info['slider'].setValue(RANGE / 2)
self.onValueChanged(event)
def loop(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
for pub in self.publishers:
self.publishers[pub].update(self.values)
rate.sleep()
def __init__(self, content):
super(SliderPublisher, self).__init__()
content = content.replace('\t', ' ')
# get message types
self.publishers = {}
self.values = {}
pkgs = []
# to keep track of key ordering in the yaml file
order = []
old = []
for topic, info in yaml.load(content).items():
pkg, msg = info['type'].split('/')
pkgs.append(__import__(pkg, globals(), locals(), ['msg']))
self.publishers[topic] = Publisher(topic,
getattr(pkgs[-1].msg,
msg), info)
order.append((topic, []))
for key in info:
self.values[key] = info[key]
order[-1][1].append((content.find(' ' + key + ':'), key))
old.append((content.find(' ' + key + ':'), key))
for bound in ['min', 'max']:
self.values[key][bound] = float(self.values[key][bound])
self.values[key]['val'] = 0
order[-1][1].sort()
order.sort(key=lambda x: x[1][0][0])
# build sliders - thanks joint_state_publisher
sliderUpdateTrigger = Signal()
self.vlayout = QVBoxLayout(self)
self.gridlayout = QGridLayout()
font = QFont("Helvetica", 9, QFont.Bold)
topic_font = QFont("Helvetica", 10, QFont.Bold)
sliders = []
self.key_map = {}
y = 0
for topic, keys in order:
topic_layout = QVBoxLayout()
label = QLabel(topic)
label.setFont(topic_font)
topic_layout.addWidget(label)
self.gridlayout.addLayout(topic_layout, *(y, 0))
y += 1
for idx, key in keys:
key_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(key)
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)
key_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
key_layout.addWidget(slider)
self.key_map[key] = {
'slidervalue': 0,
'display': display,
'slider': slider
}
slider.valueChanged.connect(self.onValueChanged)
self.gridlayout.addLayout(key_layout, *(y, 0))
y += 1
#sliders.append(key_layout)
# Generate positions in grid and place sliders there
#self.positions = [(y,0) for y in range(len(sliders))]
#for item, pos in zip(sliders, self.positions):
# self.gridlayout.addLayout(item, *pos)
self.vlayout.addLayout(self.gridlayout)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center)
self.vlayout.addWidget(self.ctrbutton)
self.center(1)
class HandEyeCalibration(Plugin):
PLUGIN_TITLE = ' Intel OTC Robotics: Hand-Eye Calibration'
def __init__(self, context):
super(HandEyeCalibration, self).__init__(context)
self.context = context
self.node = context.node
self.widget = QWidget()
self.widget.setObjectName(self.PLUGIN_TITLE)
self.widget.setWindowTitle(self.PLUGIN_TITLE)
# Data
self.Tsamples = []
# Toolbar
_, path_pkg = get_resource('packages', 'handeye_dashboard')
print("{}".format(path_pkg))
self.snapshot_action = QAction(QIcon.fromTheme('camera-photo'),
'Take a snapshot', self.widget)
path = path_pkg + '/share/handeye_dashboard/images/capture.png'
self.calibrate_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
'Get the camera/robot transform',
self.widget)
self.clear_action = QAction(QIcon.fromTheme('edit-clear'),
'Clear the record data.', self.widget)
path = path_pkg + '/share/handeye_dashboard/images/UR5.png'
self.execut_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
'EStart the publishing the TF.',
self.widget)
self.toolbar = QToolBar()
self.toolbar.addAction(self.snapshot_action)
self.toolbar.addAction(self.calibrate_action)
self.toolbar.addAction(self.clear_action)
self.toolbar.addAction(self.execut_action)
# Toolbar0
self.l0 = QLabel(self.widget)
self.l0.setText("Camera-Mount-Type: ")
self.l0.setFixedWidth(150)
self.l0.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.combobox = QComboBox(self.widget)
self.combobox.addItem('attached on robot')
self.combobox.addItem('fixed beside robot')
self.toolbar0 = QToolBar()
self.toolbar0.addWidget(self.l0)
self.toolbar0.addWidget(self.combobox)
# Toolbar1
self.l1 = QLabel(self.widget)
self.l1.setText("Camera-Frame: ")
self.l1.setFixedWidth(150)
self.l1.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.camera_frame = QLineEdit(self.widget)
self.camera_frame.setText("camera_link")
self.toolbar1 = QToolBar()
self.toolbar1.addWidget(self.l1)
self.toolbar1.addWidget(self.camera_frame)
# Toolbar2
self.l2 = QLabel(self.widget)
self.l2.setText("Object-Frame: ")
self.l2.setFixedWidth(150)
self.l2.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.object_frame = QLineEdit(self.widget)
self.object_frame.setText("calib_board")
self.toolbar2 = QToolBar()
self.toolbar2.addWidget(self.l2)
self.toolbar2.addWidget(self.object_frame)
# Toolbar3
self.l3 = QLabel(self.widget)
self.l3.setText("Robot-Base-Frame: ")
self.l3.setFixedWidth(150)
self.l3.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.base_frame = QLineEdit(self.widget)
self.base_frame.setText("base")
self.toolbar3 = QToolBar()
self.toolbar3.addWidget(self.l3)
self.toolbar3.addWidget(self.base_frame)
# Toolbar4
self.l4 = QLabel(self.widget)
self.l4.setText("End-Effector-Frame: ")
self.l4.setFixedWidth(150)
self.l4.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.endeffector_frame = QLineEdit(self.widget)
self.endeffector_frame.setText("tool0")
self.toolbar4 = QToolBar()
self.toolbar4.addWidget(self.l4)
self.toolbar4.addWidget(self.endeffector_frame)
# Toolbar5
self.l5 = QLabel(self.widget)
self.l5.setText("Sample-Number: ")
self.l5.setFixedWidth(150)
self.l5.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.le5 = QLineEdit(self.widget)
self.le5.setValidator(QIntValidator())
self.le5.setText('10')
self.le5.setReadOnly(True)
self.toolbar5 = QToolBar()
self.toolbar5.addWidget(self.l5)
self.toolbar5.addWidget(self.le5)
# TreeView
self.treeview = QTreeView()
self.treeview.setAlternatingRowColors(True)
self.model = QStandardItemModel(self.treeview)
self.treeview.setModel(self.model)
self.treeview.setHeaderHidden(True)
# TextEdit
self.textedit = QTextEdit(self.widget)
self.textedit.setReadOnly(True)
# Layout
self.layout = QVBoxLayout()
self.layout.addWidget(self.toolbar0)
self.layout.addWidget(self.toolbar1)
self.layout.addWidget(self.toolbar2)
self.layout.addWidget(self.toolbar3)
self.layout.addWidget(self.toolbar4)
self.layout.addWidget(self.toolbar5)
self.layout.addWidget(self.toolbar)
self.layoutH = QHBoxLayout()
self.layoutH.addWidget(self.treeview)
self.layoutH.addWidget(self.textedit)
self.layout.addLayout(self.layoutH)
self.widget.setLayout(self.layout)
# Add the widget to the user interface
if context.serial_number() > 1:
self.widget.setWindowTitle(self.widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self.widget)
# Make the connections
self.snapshot_action.triggered.connect(self.take_snapshot)
self.calibrate_action.triggered.connect(self.calibration)
self.clear_action.triggered.connect(self.clear)
self.execut_action.triggered.connect(self.execution)
# Package path
self.path_pkg = path_pkg
# Set up TF
self.cli = self.node.create_client(HandeyeTF, 'handeye_tf_service')
while not self.cli.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'service not available, waiting again...')
self.req = HandeyeTF.Request()
def clear(self):
# >>> Clear the recorded samples
self.textedit.append('Clearing the recorded data ...')
self.textedit.clear()
self.Tsamples = []
self.model.clear()
def get_tf_transform(self, frame_id, child_frame_id):
self.req.transform.header.frame_id = frame_id
self.req.transform.child_frame_id = child_frame_id
self.req.publish.data = False
future = self.cli.call_async(self.req)
rclpy.spin_until_future_complete(self.node, future)
transform = TransformStamped()
try:
result = future.result()
except Exception as e:
self.node.get_logger().info('Service call failed %r' % (e, ))
else:
transform = result.tf_lookup_result
return transform
def publish_tf_transform(self, transform_to_publish):
self.req.publish.data = True
self.req.transform = transform_to_publish
future = self.cli.call_async(self.req)
rclpy.spin_until_future_complete(self.node, future)
try:
future.result()
except Exception as e:
self.node.get_logger().info('Service call failed %r' % (e, ))
else:
self.node.get_logger().info(
'Send the camera-robot transform :\n\tfrom `{}` to `{}`.'.
format(self.req.transform.header.frame_id,
self.req.transform.child_frame_id))
def take_snapshot(self):
# >>> Take the snapshot
self.textedit.append('Taking snapshot ...')
# Get the transform from `tool0` to `base_link`
T = self.get_tf_transform(self.base_frame.text(),
self.endeffector_frame.text())
bTe = np.zeros((4, 4))
q = [
T.transform.rotation.w, T.transform.rotation.x,
T.transform.rotation.y, T.transform.rotation.z
]
bTe = br.quaternion.to_transform(q)
bTe[:3, 3] = np.array([
T.transform.translation.x, T.transform.translation.y,
T.transform.translation.z
])
self.textedit.append('Lookup transform: from `{}` to `{}`.'.format(
self.base_frame.text(), self.endeffector_frame.text()))
self.node.get_logger().info(bcolors.OKGREEN + 'bTe:' + bcolors.ENDC +
'\n{}'.format(bTe))
# Get the transform from `calib_board` to `camera_link`
T = self.get_tf_transform(self.camera_frame.text(),
self.object_frame.text())
cTo = np.zeros((4, 4))
q = [
T.transform.rotation.w, T.transform.rotation.x,
T.transform.rotation.y, T.transform.rotation.z
]
cTo = br.quaternion.to_transform(q)
cTo[:3, 3] = np.array([
T.transform.translation.x, T.transform.translation.y,
T.transform.translation.z
])
self.textedit.append('Lookup transform: from `{}` to `{}`.'.format(
self.camera_frame.text(), self.object_frame.text()))
self.node.get_logger().info(bcolors.OKGREEN + 'cTo:' + bcolors.ENDC +
'\n{}'.format(cTo))
parent = QStandardItem('Snapshot {}'.format(len(self.Tsamples)))
child_1 = QStandardItem('bTe:\n{}\n{}\n{}\n{}'.format(
bTe[0, :], bTe[1, :], bTe[2, :], bTe[3, :]))
child_2 = QStandardItem('cTo:\n{}\n{}\n{}\n{}'.format(
cTo[0, :], cTo[1, :], cTo[2, :], cTo[3, :]))
parent.appendRow(child_1)
parent.appendRow(child_2)
self.model.appendRow(parent)
self.Tsamples.append((bTe, cTo))
self.le5.setText(str(len(self.Tsamples)))
def calibration(self):
# >>> Compute the calibration
self.textedit.append('Making the calibration ...')
if len(self.Tsamples) == 0:
self.textedit.append(
'No transform recorded, please take snapshots.')
return
# save samples to `dataset.json` file
save_samples_to_file(self.Tsamples)
import handeye
if self.combobox.currentIndex() == 0:
solver_cri = handeye.calibrator.HandEyeCalibrator(setup='Moving')
if self.combobox.currentIndex() == 1:
solver_cri = handeye.calibrator.HandEyeCalibrator(setup='Fixed')
for sample in self.Tsamples:
solver_cri.add_sample(sample[0], sample[1])
try:
bTc = solver_cri.solve(method=handeye.solver.Daniilidis1999)
# save the calibration result to 'camera-robot.json' file
file_output = '/tmp/' + 'camera-robot.json'
with open(file_output, 'w') as f:
json.dump(bTc.tolist(), f)
except Exception:
self.textedit.append("Failed to solve the hand-eye calibration.")
def execution(self):
# >>> Publish the camera-robot transform
self.textedit.append('Publishing the camera TF ...')
file_input = '/tmp/' + 'camera-robot.json'
with open(file_input, 'r') as f:
datastore = json.load(f)
to_frame = self.camera_frame.text()
if self.combobox.currentIndex() == 0:
from_frame = self.endeffector_frame.text()
if self.combobox.currentIndex() == 1:
from_frame = self.base_frame.text()
bTc = np.array(datastore)
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = ROSClock().now().to_msg()
static_transformStamped.header.frame_id = from_frame
static_transformStamped.child_frame_id = to_frame
static_transformStamped.transform.translation.x = bTc[0, 3]
static_transformStamped.transform.translation.y = bTc[1, 3]
static_transformStamped.transform.translation.z = bTc[2, 3]
q = br.transform.to_quaternion(bTc)
static_transformStamped.transform.rotation.x = q[1]
static_transformStamped.transform.rotation.y = q[2]
static_transformStamped.transform.rotation.z = q[3]
static_transformStamped.transform.rotation.w = q[0]
self.publish_tf_transform(static_transformStamped)
output_string = "camera-robot pose:\n"
output_string += " Translation: [{}, {}, {}]\n".format(
bTc[0, 3], bTc[1, 3], bTc[2, 3])
output_string += " Rotation: in Quaternion [{}, {}, {}, {}]".format(
q[0], q[1], q[2], q[3])
file_path = '/tmp/' + 'camera-robot.txt'
with open(file_path, 'w') as f:
f.write(output_string)
def shutdown_plugin(self):
"""
Unregister subscribers when the plugin shutdown
"""
pass
def save_settings(self, plugin_settings, instance_settings):
# Nothing to be done here
pass
def restore_settings(self, plugin_settings, instance_settings):
# Nothing to be done here
pass
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 setup(self):
layout = QGridLayout()
# Sidebar
self.benchmark_combo_box = QComboBox()
for key in self.config['benchmarks'].keys():
self.benchmark_combo_box.addItem(
self.config['benchmarks'][key]['name'])
self.benchmark_combo_box.currentIndexChanged.connect(
self._handle_benchmark_selected)
self.benchmark_combo_box.setMaximumWidth(SIDEBAR_WIDTH)
self.set_current_benchmark()
self.team_combo_box = QComboBox()
for key in self.config['teams']:
self.team_combo_box.addItem(key)
self.team_combo_box.setMaximumWidth(SIDEBAR_WIDTH)
self.test_communication_button = QPushButton('Test communication')
self.test_communication_button.clicked.connect(self._handle_test_comm)
self.trial_list_widget = QListWidget()
self.trial_list_widget.currentItemChanged.connect(
self._handle_trial_change)
self.trial_list_widget.setMaximumWidth(SIDEBAR_WIDTH)
sidebar_layout = QVBoxLayout()
sidebar_layout.addWidget(QLabel("Team"))
sidebar_layout.addWidget(self.team_combo_box)
sidebar_layout.addWidget(self.test_communication_button)
sidebar_layout.addWidget(QLabel("Benchmark"))
sidebar_layout.addWidget(self.benchmark_combo_box)
sidebar_layout.addWidget(QLabel("Trial"))
sidebar_layout.addWidget(self.trial_list_widget)
self.generate_button = QPushButton('Generate')
self.generate_button.clicked.connect(self._handle_generate)
self.delete_button = QPushButton('Delete')
self.delete_button.clicked.connect(self._handle_delete)
self.save_trials_button = QPushButton('Save')
self.save_trials_button.clicked.connect(self._handle_save_trial_config)
self.lock_button = QPushButton('Lock')
if self.config_locked:
self.lock_button.setText('Unlock')
self.lock_button.clicked.connect(self._handle_lock)
sidebar_button_layout = QGridLayout()
sidebar_button_layout.addWidget(self.generate_button, 0, 0)
sidebar_button_layout.addWidget(self.delete_button, 0, 1)
sidebar_button_layout.addWidget(self.save_trials_button, 1, 0)
sidebar_button_layout.addWidget(self.lock_button, 1, 1)
sidebar_layout.addLayout(sidebar_button_layout)
layout.addLayout(sidebar_layout, 0, 0)
# Status box
self.status = QPlainTextEdit()
self.status.setReadOnly(True)
self.status.setMaximumHeight(200)
# row 1, col 0, rowspan 1, colspan 2
layout.addWidget(self.status, 1, 0, 1, 2)
# trial config and results
trial_layout = QVBoxLayout()
self.trial_config_layout = QHBoxLayout()
self.trial_results_layout = QVBoxLayout()
self.setup_trial_config()
# benchmark trial controls
benchmark_controls_group_box = QGroupBox('Controls')
benchmark_controls_layout = QHBoxLayout()
self.start_trial_button = QPushButton('Start')
self.start_trial_button.clicked.connect(self._handle_start_trial)
self.stop_trial_button = QPushButton('Stop')
self.stop_trial_button.clicked.connect(self._handle_stop_trial)
self.prev_trial_button = QPushButton('Previous')
self.prev_trial_button.clicked.connect(self._handle_prev_trial)
self.next_trial_button = QPushButton('Next')
self.next_trial_button.clicked.connect(self._handle_next_trial)
self.start_continuous_recording_button = QPushButton(
'Start continuous recording')
self.start_continuous_recording_button.clicked.connect(
self._handle_continuous_recording)
self.timer_field = QLabel()
font = QFont("Arial", 20, QFont.Bold)
self.timer_field.setFont(font)
self.timer_field.setAutoFillBackground(True)
benchmark_controls_layout.addWidget(self.start_trial_button)
benchmark_controls_layout.addWidget(self.stop_trial_button)
benchmark_controls_layout.addWidget(self.prev_trial_button)
benchmark_controls_layout.addWidget(self.next_trial_button)
benchmark_controls_layout.addWidget(
self.start_continuous_recording_button)
benchmark_controls_layout.addWidget(self.timer_field)
benchmark_controls_group_box.setLayout(benchmark_controls_layout)
trial_layout.addLayout(self.trial_config_layout)
trial_layout.addWidget(benchmark_controls_group_box)
trial_layout.addLayout(self.trial_results_layout)
self.save_results_button = QPushButton('Save results')
self.save_results_button.clicked.connect(self._handle_save_result)
trial_layout.addWidget(self.save_results_button)
layout.addLayout(trial_layout, 0, 1)
self.setLayout(layout)
self.show()
self.show_env_var('ROS_MASTER_URI')
self.show_env_var('ROS_IP')
self.show_env_var('ROS_HOSTNAME')
