class JointStatePublisherGui(QWidget):
sliderUpdateTrigger = Signal()
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
self.vlayout = QVBoxLayout(self)
self.scrollable = QWidget()
self.gridlayout = QGridLayout()
self.scroll = QScrollArea()
self.scroll.setWidgetResizable(True)
font = QFont("Helvetica", 9, QFont.Bold)
### Generate sliders ###
sliders = []
for name in self.jsp.joint_list:
if name not in self.jsp.free_joints:
continue
joint = self.jsp.free_joints[name]
if joint['min'] == joint['max']:
continue
joint_layout = QVBoxLayout()
row_layout = QHBoxLayout()
label = QLabel(name)
label.setFont(font)
row_layout.addWidget(label)
display = QLineEdit("0.00")
display.setAlignment(Qt.AlignRight)
display.setFont(font)
display.setReadOnly(True)
row_layout.addWidget(display)
joint_layout.addLayout(row_layout)
slider = QSlider(Qt.Horizontal)
slider.setFont(font)
slider.setRange(0, RANGE)
slider.setValue(RANGE / 2)
joint_layout.addWidget(slider)
self.joint_map[name] = {
'slidervalue': 0,
'display': display,
'slider': slider,
'joint': joint
}
# Connect to the signal provided by QSignal
slider.valueChanged.connect(
lambda event, name=name: self.onValueChangedOne(name))
sliders.append(joint_layout)
# Determine number of rows to be used in grid
self.num_rows = num_rows
# if desired num of rows wasn't set, default behaviour is a vertical layout
if self.num_rows == 0:
self.num_rows = len(sliders) # equals VBoxLayout
# Generate positions in grid and place sliders there
self.positions = self.generate_grid_positions(len(sliders),
self.num_rows)
for item, pos in zip(sliders, self.positions):
self.gridlayout.addLayout(item, *pos)
# Set zero positions read from parameters
self.center()
# Synchronize slider and displayed value
self.sliderUpdate(None)
# Set up a signal for updating the sliders based on external joint info
self.sliderUpdateTrigger.connect(self.updateSliders)
self.scrollable.setLayout(self.gridlayout)
self.scroll.setWidget(self.scrollable)
self.vlayout.addWidget(self.scroll)
# Buttons for randomizing and centering sliders and
# Spinbox for on-the-fly selecting number of rows
self.randbutton = QPushButton('Randomize', self)
self.randbutton.clicked.connect(self.randomize_event)
self.vlayout.addWidget(self.randbutton)
self.ctrbutton = QPushButton('Center', self)
self.ctrbutton.clicked.connect(self.center_event)
self.vlayout.addWidget(self.ctrbutton)
self.maxrowsupdown = QSpinBox()
self.maxrowsupdown.setMinimum(1)
self.maxrowsupdown.setMaximum(len(sliders))
self.maxrowsupdown.setValue(self.num_rows)
self.maxrowsupdown.valueChanged.connect(self.reorggrid_event)
self.vlayout.addWidget(self.maxrowsupdown)
self.setLayout(self.vlayout)
def onValueChangedOne(self, name):
# A slider value was changed, but we need to change the joint_info metadata.
joint_info = self.joint_map[name]
joint_info['slidervalue'] = joint_info['slider'].value()
joint = joint_info['joint']
joint['position'] = self.sliderToValue(joint_info['slidervalue'],
joint)
joint_info['display'].setText("%.2f" % joint['position'])
@pyqtSlot()
def updateSliders(self):
self.update_sliders()
def update_sliders(self):
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(
joint['position'], joint)
joint_info['slider'].setValue(joint_info['slidervalue'])
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(joint['zero'], joint))
def reorggrid_event(self, event):
self.reorganize_grid(event)
def reorganize_grid(self, number_of_rows):
self.num_rows = number_of_rows
# Remove items from layout (won't destroy them!)
items = []
for pos in self.positions:
item = self.gridlayout.itemAtPosition(*pos)
items.append(item)
self.gridlayout.removeItem(item)
# Generate new positions for sliders and place them in their new spots
self.positions = self.generate_grid_positions(len(items),
self.num_rows)
for item, pos in zip(items, self.positions):
self.gridlayout.addLayout(item, *pos)
def generate_grid_positions(self, num_items, num_rows):
if num_rows == 0:
return []
positions = [
(y, x)
for x in range(int((math.ceil(float(num_items) / num_rows))))
for y in range(num_rows)
]
positions = positions[:num_items]
return positions
def randomize_event(self, event):
self.randomize()
def randomize(self):
rospy.loginfo("Randomizing")
for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slider'].setValue(
self.valueToSlider(random.uniform(joint['min'], joint['max']),
joint))
def sliderUpdate(self, event):
for name, joint_info in self.joint_map.items():
joint_info['slidervalue'] = joint_info['slider'].value()
self.update_sliders()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] -
joint['min'])
def sliderToValue(self, slider, joint):
pctvalue = slider / float(RANGE)
return joint['min'] + (joint['max'] - joint['min']) * pctvalue
def __init__(self, title, jsp, num_rows=0):
super(JointStatePublisherGui, self).__init__()
font = QFont("Helvetica", 9, QFont.Bold)
self.hlayout = QHBoxLayout(self)
vlayout = QVBoxLayout()
glayout = QGridLayout()
right_l_lauout = QVBoxLayout()
self.listVeiw = QListWidget()
self.checkbox = []
self.value_line_edit = []
self.sliders = []
self.positions = []
self.progressbars = []
self.value_last = []
speed_max = enviromnt_conf['joint_speed']
slider_max = speed_max * 1000
position_max = enviromnt_conf['joint_max_position']
progress_max = position_max * 1000
#create joints widget
for i in range(0, num_rows):
if config[i][0]:
g_in_g = QGridLayout()
checkbox = QCheckBox(config[i][1])
checkbox.setFont(font)
self.checkbox.append(checkbox)
value_line_edit = QLineEdit()
value_line_edit.setFont(font)
value_line_edit.setText("0.0")
self.value_line_edit.append(value_line_edit)
display_lable = QLabel()
display_lable.setFont(font)
display_lable.setText("Position:")
position_label = QLabel()
position_label.setFont(font)
position_label.setText("0.0")
self.positions.append(position_label)
position_progress_bar = QProgressBar()
position_progress_bar.setMaximum(progress_max)
position_progress_bar.setMinimum(-progress_max)
position_progress_bar.setValue(0)
self.progressbars.append(position_progress_bar)
slider = QSlider()
slider.setMaximum(slider_max)
slider.setMinimum(-slider_max)
slider.setOrientation(Qt.Horizontal)
slider.valueChanged.connect(self.slider_value_changed)
self.sliders.append(slider)
g_in_g.addWidget(checkbox, 0, 0)
g_in_g.addWidget(value_line_edit, 0, 1)
g_in_g.addWidget(display_lable, 0, 2)
g_in_g.addWidget(position_label, 0, 3)
g_in_g.addWidget(slider, 1, 0, 1, 2)
g_in_g.addWidget(position_progress_bar, 1, 2, 1, 2)
glayout.addLayout(g_in_g, i, 0)
#create v layout
self.import_Btn = QPushButton('Import')
self.import_Btn.setFont(font)
self.import_Btn.clicked.connect(self.import_Btn_clecked)
self.export_Btn = QPushButton('Export')
self.export_Btn.setFont(font)
self.export_Btn.clicked.connect(self.export_Btn_clicked)
self.start_Btn = QPushButton("Start")
self.start_Btn.setFont(font)
self.start_Btn.clicked.connect(self.start_Btn_clicked)
self.reset_Btn = QPushButton('Reset')
self.reset_Btn.setFont(font)
self.reset_Btn.clicked.connect(self.reset_Btn_clicked)
self.record_Btn = QPushButton('Record')
self.record_Btn.setFont(font)
self.record_Btn.clicked.connect(self.record_Btn_clicked)
self.replay_Btn = QPushButton('Repaly')
self.replay_Btn.setFont(font)
self.replay_Btn.clicked.connect(self.replay_Btn_clicked)
self.delete_Btn = QPushButton("Delete")
self.delete_Btn.setFont(font)
self.delete_Btn.clicked.connect(self.delete_Btn_clicked)
self.debug_Btn = QPushButton("Debug")
self.debug_Btn.setFont(font)
self.debug_Btn.clicked.connect(self.debug_Btn_clicked)
vlayout.addWidget(self.import_Btn)
vlayout.addWidget(self.export_Btn)
vlayout.addWidget(self.start_Btn)
vlayout.addWidget(self.reset_Btn)
vlayout.addWidget(self.record_Btn)
vlayout.addWidget(self.delete_Btn)
vlayout.addWidget(self.replay_Btn)
vlayout.addWidget(self.debug_Btn)
self.master_url = QLineEdit("http://192.168.0.91:11311")
self.master_url.setFont(font)
self.master_ip = QLineEdit("192.168.0.91")
self.master_ip.setFont(font)
self.listVeiw.clicked.connect(self.listVeiw_clicked)
self.listVeiw.currentRowChanged.connect(
self.listVeiw_itemSelectionChanged)
self.description = QTextEdit("")
self.description.setFont(font)
#self.description.setGeometry(0,100,100,500)
right_l_lauout.addWidget(self.master_url)
right_l_lauout.addWidget(self.master_ip)
right_l_lauout.addWidget(self.listVeiw)
right_l_lauout.addWidget(self.description)
right_l_lauout.setStretch(0, 1)
right_l_lauout.setStretch(1, 1)
right_l_lauout.setStretch(2, 3)
right_l_lauout.setStretch(3, 1)
self.num_rows = len(self.checkbox)
self.hlayout.addLayout(glayout)
self.hlayout.addLayout(vlayout)
self.hlayout.addLayout(right_l_lauout)
self.setLayout(self.hlayout)
self.callback_start = None
self.callback_pause = None
self.callback_record = None
self.callback_reset = None
self.callback_replay = None
self.callback_replay_stop = None
self.callback_delete = None
self.callback_debug = None
self.callback_import = None
self.callback_export = None
self.callback_list_clicked = None
self.listVeiw_isClicked = False
self.listVeiw_current_item = 0
self.listVeiw_len = 0
self.f = QFileDialog()
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()
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 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')