def add_config(self, title, choices, single_choice=True):
'''
create a UI element for selecting options for one variation
and put it in a scrollArea
'''
scroll = QScrollArea()
group_box = QGroupBox(title)
group_box.setFlat(True)
layout = QVBoxLayout()
if len(choices) > 5 and single_choice:
combo_box = QComboBox(group_box)
for obj in choices:
combo_box.addItem(obj)
layout.addWidget(combo_box)
else:
for obj in choices:
if single_choice:
layout.addWidget(QRadioButton(obj, group_box))
else:
layout.addWidget(QCheckBox(obj, group_box))
layout.addStretch(1)
group_box.setLayout(layout)
scroll.setWidget(group_box)
scroll.setWidgetResizable(True)
return group_box, scroll
class LineEditDialog(QDialog):
def __init__(self, parent=None):
super(LineEditDialog, self).__init__()
self.value = None
vbox = QVBoxLayout(self)
# combo box
model = QtGui.QStandardItemModel(self)
for elm in rospy.get_param_names():
model.setItem(model.rowCount(), 0, QtGui.QStandardItem(elm))
self.combo_box = QComboBox(self)
self.line_edit = QLineEdit()
self.combo_box.setLineEdit(self.line_edit)
self.combo_box.setCompleter(QCompleter())
self.combo_box.setModel(model)
self.combo_box.completer().setModel(model)
self.combo_box.lineEdit().setText('')
vbox.addWidget(self.combo_box)
# button
button = QPushButton()
button.setText("Done")
button.clicked.connect(self.buttonCallback)
vbox.addWidget(button)
self.setLayout(vbox)
def buttonCallback(self, event):
self.value = self.line_edit.text()
self.close()
def __init__(self, parent=None):
super(ComboBoxDialog, self).__init__()
self.number = 0
vbox = QVBoxLayout(self)
self.combo_box = QComboBox(self)
self.combo_box.activated.connect(self.onActivated)
vbox.addWidget(self.combo_box)
button = QPushButton()
button.setText("Done")
button.clicked.connect(self.buttonCallback)
vbox.addWidget(button)
self.setLayout(vbox)
def _add_parameter(self, param):
key = QTableWidgetItem(param.key)
key.setFlags(key.flags() & ~Qt.ItemIsEditable)
row = self.skill_params_table.rowCount()
self.skill_params_table.insertRow(row)
#row = row-1
self.skill_params_table.setItem(row, 0, key)
if param.dataTypeIs(bool):
cbox = QCheckBox()
if param.hasSpecifiedDefault():
cbox.setChecked(param.default)
self.skill_params_table.setCellWidget(row, 1, cbox)
elif param.dataTypeIs(Element):
combobox = QComboBox()
self.skill_params_table.setCellWidget(row, 1, combobox)
matches = self._wmi.resolve_elements(param.default)
if param.paramTypeIs(ParamTypes.Optional):
combobox.addItem("", None)
for e in matches:
combobox.addItem(e.printState(), e._id)
else:
lineedit = QLineEdit()
if param.isSpecified():
lineedit.setText(str(param.value))
self.skill_params_table.setCellWidget(row, 1, lineedit)
def add_combobox_empty(self, name, title, curtext=""):
if title:
self.add_label(title)
combo = QComboBox()
combo.setObjectName(name)
combo.addItem(curtext)
self.group_area.layout().addWidget(combo)
self.empty_combo = combo
def create_dropdown(self, options, selection):
try:
index = options.index(selection)
except ValueError:
rospy.loginfo('Selection %s not found in options %s.', str(selection), str(options))
index = -1
dropdown = QComboBox()
for option in options:
dropdown.addItem(option)
dropdown.setCurrentIndex(index)
return dropdown
def add_combobox(self, name, options, current, title="", on_change=None):
if title:
self.add_label(title)
combo = QComboBox()
combo.setObjectName(name)
for op in options:
combo.addItem(op)
cur_ind = 0
if current in options:
cur_ind = options.index(current)
combo.setCurrentIndex(cur_ind)
if on_change is not None:
combo.activated.connect(on_change)
self.group_area.layout().addWidget(combo)
def create_comboBox(self, subtype='sumo:Object', label='Subtype'):
"""Inserts a new combobox in the dialog based on the subtype.
Helper function that creates a combobox and fills the list with filtered items from the ontology/world model.
Args:
subtype (str, optional): Type to be used to retrieve world model items for the dropdown list
label (str, optional): Label for the dropdown list
"""
comboBox = QComboBox()
sizePolicy = QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed)
comboBox.setSizePolicy(sizePolicy)
comboBox.addItem('All')
[comboBox.addItem(l, d) for l, d in self.get_types(subtype).iteritems()]
comboBox.currentIndexChanged.connect(partial(self.on_select_type, len(self._comboBoxes)))
self.formLayout.insertRow(len(self._comboBoxes), label, comboBox)
self._comboBoxes.append(comboBox)
class MetricsRefboxWidget(QWidget):
status_signal = pyqtSignal(object)
timeout_signal = pyqtSignal(object)
result_signal = pyqtSignal(object)
bagfile_signal = pyqtSignal(object)
def __init__(self):
super(MetricsRefboxWidget, self).__init__()
self.status_signal.connect(self.update_status)
self.timeout_signal.connect(self._handle_timeout)
self.result_signal.connect(self.update_result)
self.bagfile_signal.connect(self._update_bagfile_name)
self.timer = QElapsedTimer()
self.timer_interrupt = QTimer(self)
self.timer_interrupt.timeout.connect(self.update_timer)
self.timer_interrupt.start(100)
self.metrics_refbox = metrics_refbox.MetricsRefbox(
self._status_cb, self._start_cb, self._result_cb)
self.trial_configs = {}
self.current_benchmark = None
self.current_benchmark_enum = -1
# once we receive confirmation from robot
self.benchmark_running = False
# once we send the start message to robot
self.benchmark_started = False
# set to True if we're not recording individual trials
# but rather continuously recording all data (hence no timeouts)
self.continuous_recording = False
self.timeout = False
self.stopped = False
self.result_msg = None
self.config_locked = True
self.config = self.metrics_refbox.get_config()
self.setup()
self.show()
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')
def show_env_var(self, var):
env_str = os.getenv(var) if os.getenv(var) is not None else ''
msg = var + ': ' + env_str + '\n'
self.update_status(msg)
def set_current_benchmark(self):
'''
Sets the current benchmark type based on user selection
Load config file for selected benchmark and setup GUI to show config and results based on that
'''
benchmark_index = self.benchmark_combo_box.currentIndex()
benchmark_name = list(
self.config['benchmarks'].keys())[benchmark_index]
benchmark_result_type = getattr(
metrics_refbox_msgs.msg,
self.config['benchmarks'][benchmark_name]['type'])
config_file_name = benchmark_name + '.json'
config_file = os.path.join(self.metrics_refbox.get_config_file_path(),
config_file_name)
stream = open(config_file, 'r')
benchmark_config = json.load(stream)
module = importlib.import_module(benchmark_config['module'])
self.current_benchmark = getattr(module, benchmark_config['class'])(
benchmark_config, self.metrics_refbox.get_config_file_path(),
benchmark_name, benchmark_result_type)
self.current_benchmark_enum = self.config['benchmarks'][
benchmark_name]['enum']
def setup_trial_config(self):
'''
set up the benchmark specific part of the GUI related to the configuration of
the benchmark (for example, which objects will be used for object detection)
the specific configuration is loaded based on the selected trial_index
'''
for i in reversed(range(self.trial_config_layout.count())):
self.trial_config_layout.itemAt(i).widget().setParent(None)
for i in reversed(range(self.trial_results_layout.count())):
self.trial_results_layout.itemAt(i).widget().setParent(None)
self.current_benchmark.setup(self.trial_config_layout,
self.trial_results_layout,
self.config_locked)
self.trial_list_widget.clear()
self.trial_configs.clear()
path = os.path.join(self.metrics_refbox.get_config_file_path(),
'trials')
trial_config_file = os.path.join(
path, self.current_benchmark.config['trial_config'])
if os.path.exists(trial_config_file):
with open(trial_config_file, "r") as fp:
trial_config = json.load(fp)
for key in sorted(trial_config.keys()):
trial_item = QListWidgetItem(key)
trial_item.setFlags(trial_item.flags() | Qt.ItemIsEditable)
self.trial_list_widget.addItem(trial_item)
self.trial_configs[key] = trial_config[key]
self.trial_list_widget.setCurrentRow(0)
def _handle_test_comm(self):
'''
'Test communication' button
'''
self.metrics_refbox.test_communication()
self.status_signal.emit('Sent test message to all clients\n')
def _handle_benchmark_selected(self, idx):
'''
benchmark selection has changed
'''
self._handle_save_trial_config(None)
self.set_current_benchmark()
self.setup_trial_config()
def _handle_trial_change(self, current_item, previous_item):
'''
Trial selection has changed
'''
if previous_item is not None:
self.trial_configs[previous_item.text(
)] = self.current_benchmark.get_current_selections()
self.current_benchmark.clear_results()
if current_item is not None and current_item.text(
) in self.trial_configs.keys() and self.trial_configs[
current_item.text()] is not None:
self.current_benchmark.apply_selections(
self.trial_configs[current_item.text()])
else:
self.current_benchmark.clear_selections()
def _handle_generate(self, button):
'''
generate a new trial config
'''
trial_config = self.current_benchmark.generate()
names = []
for key in self.trial_configs.keys():
names.append(key)
msg = 'Select a name for this trial configuration'
text = ''
while True:
text, ok = QInputDialog.getText(self, 'Trial name', msg)
if text not in names:
break
QMessageBox.critical(self, "Error", "Name exists already")
if ok:
trial_item = QListWidgetItem(text)
trial_item.setFlags(trial_item.flags() | Qt.ItemIsEditable)
self.trial_list_widget.addItem(trial_item)
self.trial_configs[text] = trial_config
self.trial_list_widget.setCurrentItem(trial_item)
def _handle_delete(self, button):
'''
delete current trial config
'''
selected_items = self.trial_list_widget.selectedItems()
for item in selected_items:
self.trial_list_widget.takeItem(self.trial_list_widget.row(item))
del self.trial_configs[item.text()]
def _handle_save_trial_config(self, button):
'''
save trial config in case it has been edited
'''
if self.trial_list_widget.currentItem() is not None:
self.trial_configs[self.trial_list_widget.currentItem().text(
)] = self.current_benchmark.get_current_selections()
path = os.path.join(self.metrics_refbox.get_config_file_path(),
'trials')
trial_config_file = os.path.join(
path, self.current_benchmark.config['trial_config'])
with open(trial_config_file, "w") as fp:
json.dump(self.trial_configs, fp)
def _handle_lock(self, button):
'''
Lock trial config settings so they are not changed accidentally
'''
if self.config_locked:
self.current_benchmark.unlock_config()
self.config_locked = False
self.lock_button.setText('Lock')
else:
self.current_benchmark.lock_config()
self.config_locked = True
self.lock_button.setText('Unlock')
def _handle_start_trial(self, button):
'''
Trial start button; send command to refbox client via ROS node
'''
if self.benchmark_running or self.benchmark_started:
self.update_status(
"Benchmark has already started. Stop and start the benchmark if you want to restart it\n"
)
else:
self.metrics_refbox.start(
self.current_benchmark_enum,
self.current_benchmark.get_task_info_for_robot())
self.benchmark_started = True
self.current_benchmark.clear_results()
self.update_status("Sent start command\n")
self.timer_field.setText('')
self.disable_buttons()
def _handle_stop_trial(self, button):
'''
Trial stop button
'''
self.metrics_refbox.stop()
self.elapsed_time = self.timer.elapsed()
self.stopped = True
self.benchmark_running = False
self.benchmark_started = False
self.status_signal.emit("Sent stop command\n")
self.result_signal.emit(None)
self.enable_buttons()
def _handle_next_trial(self, button):
'''
Select next trial
'''
row = self.trial_list_widget.currentRow()
if (row + 1 < self.trial_list_widget.count()):
self.trial_list_widget.setCurrentRow(row + 1)
def _handle_prev_trial(self, button):
'''
Select previous trial
'''
row = self.trial_list_widget.currentRow()
if (row - 1 >= 0):
self.trial_list_widget.setCurrentRow(row - 1)
def _handle_continuous_recording(self, button):
'''
Start recording button, not tied to any benchmark, and requires user to stop recording
'''
if self.benchmark_running or self.benchmark_started:
self.metrics_refbox.stop()
self.elapsed_time = self.timer.elapsed()
self.stopped = True
self.benchmark_running = False
self.benchmark_started = False
self.status_signal.emit("Sent stop command\n")
self.start_continuous_recording_button.setText(
'Start continuous recording')
self.enable_buttons()
self.stop_trial_button.setEnabled(True)
self.continuous_recording = False
else:
self.metrics_refbox.start_recording()
self.continuous_recording = True
self.start_continuous_recording_button.setText('Stop recording')
self.benchmark_started = True
self.update_status("Sent start command\n")
self.timer_field.setText('')
self.disable_buttons()
self.stop_trial_button.setEnabled(False)
def _handle_save_result(self, button):
'''
Save results again in case notes were added
'''
self.result_signal.emit(self.result_msg)
def _start_cb(self, msg):
'''
called when we get confirmation that the robot received the start message;
official start of trial time
'''
if msg.data == True:
self.elapsed_time = 0.0
self.timer.start()
self.benchmark_running = True
self.timeout = False
self.stopped = False
self.result_msg = None
if msg.rosbag_filename == '':
self.status_signal.emit(
'Error: rosbag filename not specified although start message confirmed\n'
)
self._handle_stop_trial(None)
return
self.status_signal.emit('Started trial and recording to %s\n' %
msg.rosbag_filename)
self.bagfile_signal.emit(msg.rosbag_filename)
else:
self.status_signal.emit(
'Error: Benchmark did not start, probably because the rosbag recorder is not running\n'
)
self._handle_stop_trial(None)
def _result_cb(self, msg):
'''
called when we get a result from the robot
official end of trial time
'''
if self.benchmark_running:
self.elapsed_time = self.timer.elapsed()
self.benchmark_running = False
self.benchmark_started = False
self.status_signal.emit("Trial completed in %.2f seconds\n" %
(self.elapsed_time / 1000.0))
self.result_msg = msg
self.result_signal.emit(msg)
self.enable_buttons()
def _status_cb(self, msg):
'''
callback to send text to status box
'''
self.status_signal.emit(msg)
def update_status(self, msg):
'''
signal handler for status box
'''
timestamp = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
m = '[' + timestamp + '] ' + msg
self.status.insertPlainText(m)
self.status.ensureCursorVisible()
def _update_bagfile_name(self, name):
self.current_benchmark.set_bagfile_name(name)
def update_result(self, msg):
'''
signal handler for result message; process and save result
'''
self.current_benchmark.show_results(msg, self.timeout, self.stopped)
current_trial_name = self.trial_list_widget.currentItem().text()
current_team_name = self.team_combo_box.currentText()
results_dict = self.current_benchmark.get_trial_result_dict(
msg, current_trial_name, current_team_name, self.timeout,
self.stopped, self.elapsed_time / 1000.0)
filename = self.current_benchmark.get_bagfile_name(
)[:-4] + '_' + self.current_benchmark.benchmark_name + '.json'
path = os.path.join(self.metrics_refbox.get_results_file_path(),
filename)
with open(path, "w") as fp:
json.dump(results_dict, fp)
def update_timer(self):
'''
Timer callback; update GUI, and also check if timeout has expired
'''
if not self.benchmark_running:
return
self.timer_field.setText("Time: %.1f s" %
(self.timer.elapsed() / 1000.0))
# if we're in continuous recording mode, no need to check timeout
if self.continuous_recording:
return
if self.timer.hasExpired(self.current_benchmark.get_timeout() *
1000.0):
self.status_signal.emit(
"Trial timeout of %.2f seconds has expired\n" %
self.current_benchmark.get_timeout())
self.timeout_signal.emit('timeout expired')
def _handle_timeout(self, msg):
'''
timeout has expired, so stop the trial
'''
self.timeout = True
self._handle_stop_trial(None)
def closeEvent(self, event):
'''
make sure we save trial configs when window is closed
'''
self.metrics_refbox.stop()
self._handle_save_trial_config(None)
event.accept()
def disable_buttons(self):
'''
disable buttons when trial is running
'''
self.team_combo_box.setEnabled(False)
self.benchmark_combo_box.setEnabled(False)
self.trial_list_widget.setEnabled(False)
self.next_trial_button.setEnabled(False)
self.prev_trial_button.setEnabled(False)
self.delete_button.setEnabled(False)
self.generate_button.setEnabled(False)
self.save_trials_button.setEnabled(False)
self.lock_button.setEnabled(False)
self.start_trial_button.setEnabled(False)
def enable_buttons(self):
self.team_combo_box.setEnabled(True)
self.benchmark_combo_box.setEnabled(True)
self.trial_list_widget.setEnabled(True)
self.next_trial_button.setEnabled(True)
self.prev_trial_button.setEnabled(True)
self.delete_button.setEnabled(True)
self.generate_button.setEnabled(True)
self.save_trials_button.setEnabled(True)
self.lock_button.setEnabled(True)
self.start_trial_button.setEnabled(True)
class RqtHandeyeCalibration(Plugin):
def __init__(self, context):
super(RqtHandeyeCalibration, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('HandeyeCalibration')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print('arguments: ', args)
print('unknowns: ', unknowns)
# Create QWidgets
self._widget = QWidget()
self._infoWidget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_easy_handeye'),
'resource', 'rqt_handeye.ui')
ui_info_file = os.path.join(
rospkg.RosPack().get_path('rqt_easy_handeye'), 'resource',
'rqt_handeye_info.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
loadUi(ui_info_file, self._infoWidget)
self._widget.horizontalLayout_infoAndActions.insertWidget(
0, self._infoWidget)
self._calibration_algorithm_combobox = QComboBox()
self._calibration_algorithm_layout = QHBoxLayout()
self._calibration_algorithm_layout.insertWidget(
0, QLabel('Calibration algorithm: '))
self._calibration_algorithm_layout.insertWidget(
1, self._calibration_algorithm_combobox)
self._infoWidget.layout().insertLayout(
-1, self._calibration_algorithm_layout)
# Give QObjects reasonable names
self._widget.setObjectName('RqtHandeyeCalibrationUI')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.client = HandeyeClient()
resp = self.client.list_algorithms()
for i, a in enumerate(resp.algorithms):
self._calibration_algorithm_combobox.insertItem(i, a)
index_of_curr_alg = resp.algorithms.index(resp.current_algorithm)
self._calibration_algorithm_combobox.setCurrentIndex(index_of_curr_alg)
self._calibration_algorithm_combobox.currentTextChanged.connect(
self.client.set_algorithm)
self._infoWidget.calibNameLineEdit.setText(rospy.get_namespace())
self._infoWidget.trackingBaseFrameLineEdit.setText(
self.client.parameters.tracking_base_frame)
self._infoWidget.trackingMarkerFrameLineEdit.setText(
self.client.parameters.tracking_marker_frame)
self._infoWidget.robotBaseFrameLineEdit.setText(
self.client.parameters.robot_base_frame)
self._infoWidget.robotEffectorFrameLineEdit.setText(
self.client.parameters.robot_effector_frame)
if self.client.parameters.eye_on_hand:
self._infoWidget.calibTypeLineEdit.setText("eye on hand")
else:
self._infoWidget.calibTypeLineEdit.setText("eye on base")
self._widget.takeButton.clicked[bool].connect(self.handle_take_sample)
self._widget.removeButton.clicked[bool].connect(
self.handle_remove_sample)
self._widget.computeButton.clicked[bool].connect(
self.handle_compute_calibration)
self._widget.saveButton.clicked[bool].connect(
self.handle_save_calibration)
self._widget.removeButton.setEnabled(False)
self._widget.computeButton.setEnabled(False)
self._widget.saveButton.setEnabled(False)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
# def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def _display_sample_list(self, sample_list):
self._widget.sampleListWidget.clear()
for i in range(len(sample_list.hand_world_samples)):
formatted_robot_sample = format_sample(
sample_list.hand_world_samples[i])
formatted_tracking_sample = format_sample(
sample_list.camera_marker_samples[i])
self._widget.sampleListWidget.addItem(
'{}) \n hand->world \n {} \n camera->marker\n {}\n'.format(
i + 1, formatted_robot_sample, formatted_tracking_sample))
self._widget.sampleListWidget.setCurrentRow(
len(sample_list.hand_world_samples) - 1)
self._widget.removeButton.setEnabled(
len(sample_list.hand_world_samples) > 0)
def handle_take_sample(self):
sample_list = self.client.take_sample()
self._display_sample_list(sample_list)
self._widget.computeButton.setEnabled(
len(sample_list.hand_world_samples) > 1)
self._widget.saveButton.setEnabled(False)
def handle_remove_sample(self):
index = self._widget.sampleListWidget.currentRow()
sample_list = self.client.remove_sample(index)
self._display_sample_list(sample_list)
self._widget.computeButton.setEnabled(
len(sample_list.hand_world_samples) > 1)
self._widget.saveButton.setEnabled(False)
def handle_compute_calibration(self):
result = self.client.compute_calibration()
self._widget.computeButton.setEnabled(False)
if result.valid:
self._widget.outputBox.setPlainText(
str(result.calibration.transform.transform))
self._widget.saveButton.setEnabled(True)
else:
self._widget.outputBox.setPlainText(
'The calibration could not be computed')
self._widget.saveButton.setEnabled(False)
def handle_save_calibration(self):
self.client.save()
self._widget.saveButton.setEnabled(False)
import rclpy
rclpy.init()
topic_completer_node = rclpy.create_node()
app = QApplication(sys.argv)
mw = QMainWindow()
widget = QWidget(mw)
layout = QVBoxLayout(widget)
edit = QLineEdit()
edit_completer = TopicCompleter(edit)
edit_completer.update_topics(topic_completer_node)
# edit_completer.setCompletionMode(QCompleter.InlineCompletion)
edit.setCompleter(edit_completer)
combo = QComboBox()
combo.setEditable(True)
combo_completer = TopicCompleter(combo)
combo_completer.update_topics(topic_completer_node)
# combo_completer.setCompletionMode(QCompleter.InlineCompletion)
combo.lineEdit().setCompleter(combo_completer)
model_tree = QTreeView()
model_tree.setModel(combo_completer.model())
model_tree.expandAll()
for column in range(combo_completer.model().columnCount()):
model_tree.resizeColumnToContents(column)
completion_tree = QTreeView()
completion_tree.setModel(combo_completer.completionModel())
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')
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()
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
class HardwareUI(Plugin):
"""Backend for the GUI. Receives UDP data and distributes it across all GUI elements."""
tetrigger = QtCore.pyqtSignal()
totrigger = QtCore.pyqtSignal()
votrigger = QtCore.pyqtSignal()
imutrigger = QtCore.pyqtSignal(Imu)
def __init__(self, context):
"""Loads up the master.ui GUI file and adds some additional widgets that cannot be added beforehand. Also sets some necessary variables."""
super(HardwareUI, self).__init__(context)
self._widget = QMainWindow()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('bitbots_hardware_rqt'), 'resource',
'master.ui')
loadUi(ui_file, self._widget, {})
self.current_tab = 0
self.current_outer_tab = 0
self.historydict = []
self.colorlist = [
] #Holds information about the graph colors, so we can distinguish between the different motors
self.motornames = [
'HeadPan', 'HeadTilt', 'LAnklePitch', 'LAnkleRoll', 'LElbow',
'LHipPitch', 'LHipRoll', 'LHipYaw', 'LKnee', 'LShoulderPitch',
'LShoulderRoll', 'RAnklePitch', 'RAnkleRoll', 'RElbow',
'RHipPitch', 'RHipRoll', 'RHipYaw', 'RKnee', 'RShoulderPitch',
'RShoulderRoll'
] #We need the motor names in alphabetical order for some gui elements
for i in range(0, 20):
self.colorlist.append(
(random.randrange(50, 255), random.randrange(0, 255),
random.randrange(0, 255)))
self.templist = [[] for i in range(20)]
self.torquelist = [[] for i in range(20)]
self.voltagelist = [[] for i in range(20)]
self.crosshair = QLabel()
self.pixmap = QtGui.QPixmap(
os.path.join(rp.get_path('bitbots_hardware_rqt'), 'resource',
'cross.png'))
self.crosshair.setPixmap(self.pixmap)
self.crosshair.setParent(self._widget.label_4)
self.crosshair.move(187, 187)
self.crosshair2 = QLabel()
self.pixmap = QtGui.QPixmap(
os.path.join(rp.get_path('bitbots_hardware_rqt'), 'resource',
'cross.png'))
self.crosshair2.setPixmap(self.pixmap)
self.crosshair2.setParent(self._widget.label_11)
self.crosshair2.move(87, 87)
self.make_Graphs()
self.votrigger.connect(lambda: self.update_graph(
self.voltageplt, self.voltagelist, self.combobox2, 3))
self.tetrigger.connect(lambda: self.update_graph(
self.tempplt, self.templist, self.combobox3, 1))
self.totrigger.connect(lambda: self.update_graph(
self.torqueplt, self.torquelist, self.combobox, 2))
#self._robot_ip = [l for l in ([ip for ip in socket.gethostbyname_ex(socket.gethostname())[2] if not ip.startswith("127.")][:1], [[(s.connect(('8.8.8.8', 53)), s.getsockname()[0], s.close()) for s in [socket.socket(socket.AF_INET, socket.SOCK_DGRAM)]][0][1]]) if l][0][0]
self._robot, button_pressed = QInputDialog.getText(self._widget, "Robot?", \
"Select your Robot. Example: nuc1", QLineEdit.Normal, "")
self.diagnostics = rospy.Subscriber(
"/{}/diagnostics".format(self._robot), DiagnosticArray,
self.set_motor_diagnostics)
self.joint_states = rospy.Subscriber(
"/{}/joint_states".format(self._robot), JointState,
self.set_motor_joint_states)
self.buttons = rospy.Subscriber("/{}/buttons".format(self._robot),
Buttons, self.set_buttons)
self.imu = rospy.Subscriber("/{}/imu/data".format(self._robot), Imu,
self.emit_imu_trigger)
self.state = rospy.Subscriber("/{}/robot_state".format(self._robot),
RobotControlState, self.set_robot_state)
self.imutrigger.connect(self.set_imu)
#self.rcvthread = ReceiverThread(self._robot_ip, self._robot_port)
#self.rcvthread.start()
self._widget.GraphenView.currentChanged.connect(
self.current_graph_changed)
self._widget.tabWidget.currentChanged.connect(
self.current_outer_tab_changed)
#self._widget.label_14.setText("IP: "+ [l for l in ([ip for ip in socket.gethostbyname_ex(socket.gethostname())[2] if not ip.startswith("127.")][:1], [[(s.connect(('8.8.8.8', 53)), s.getsockname()[0], s.close()) for s in [socket.socket(socket.AF_INET, socket.SOCK_DGRAM)]][0][1]]) if l][0][0])
context.add_widget(self._widget)
self._widget.show()
def calcPosInCicle(self, radOrienation):
"""return a position on the circle in the GUI"""
yPos = -math.cos(radOrienation) * 100 + 87
xPos = math.sin(radOrienation) * 100 + 87
return xPos, yPos
def current_graph_changed(self, change):
"""Handles the changing between tabs, so that only the active tab is updated, to reduce lag"""
self.current_tab = change
def current_outer_tab_changed(self, change):
self.current_outer_tab = change
def emit_imu_trigger(self, data):
self.imutrigger.emit(data)
def set_imu(self, data):
"""Updates the IMU/Gyro widgets."""
if self.current_outer_tab == 2:
self._widget.label_8.setText(
str(math.degrees(data.angular_velocity.x)))
self._widget.label_9.setText(
str(math.degrees(data.angular_velocity.y)))
self._widget.label_10.setText(
str(math.degrees(data.angular_velocity.z)))
self.crosshair.move((187+(math.degrees(data.angular_velocity.x)*400/360)), \
(187+(math.degrees(data.angular_velocity.y)*400/360))) #To place the crosshair on the circle, circle diametre is 400px, crosshair is 25px wide
yawx, yawy = self.calcPosInCicle(data.angular_velocity.z)
self.crosshair2.move(yawx, yawy)
def set_buttons(self, data):
"""Updates the widgets placed in the topbar"""
self._widget.checkBox.setCheckState(data.button1)
self._widget.checkBox.setCheckState(data.button2)
def set_robot_state(self, data):
states = {
0: "CONTROLLABLE",
1: "FALLING",
2: "FALLEN",
3: "GETTING UP",
4: "ANIMATION RUNNING",
5: "STARTUP",
6: "SHUTDOWN",
7: "PENALTY",
8: "PENALTY ANIM",
9: "RECORD",
10: "WALKING",
11: "MOTOR OFF",
12: "HCM OFF",
13: "HARDWARE PROBLEM",
14: "PICKED UP"
}
self._widget.label_14.setText(states[data.state])
def set_motor_diagnostics(self, data):
"""Updates the table in the motor overview tab"""
self.timestamp = data.header.stamp.secs #setting timestamp here, because this topic should always exist.
self._widget.label_13.setText(str(self.timestamp))
self.motorheaderlist = []
for i in range(len(data.status)):
if data.status[i].level == 1:
self.templist[int(data.status[i].hardware_id) - 101].append(
float(data.status[i].values[3].value))
self.voltagelist[int(data.status[i].hardware_id) - 101].append(
float(data.status[i].values[1].value))
if len(self.templist[i]) > 20:
self.templist[i].pop(0)
if len(self.voltagelist[i]) > 20:
self.voltagelist[i].pop(0)
if self.current_tab == 0:
if data.status[i].level == 1:
if not data.status[i].values[
2].value in self.motorheaderlist:
self.motorheaderlist.append(
data.status[i].values[2].value)
for j in range(0, len(self.motorheaderlist)):
if j < 10:
selected = self._widget.tableWidget
k = j
else:
selected = self._widget.tableWidget_2
k = j - 10
try:
if self.motorheaderlist[j] == data.status[
i].values[2].value:
selected.setItem(
0, k,
QTableWidgetItem(
data.status[i].message))
selected.setItem(
1, k,
QTableWidgetItem(
str(
round(
float(data.status[i].
values[3].value),
2))))
selected.setItem(
3, k,
QTableWidgetItem(
str(
round(
float(data.status[i].
values[1].value),
2))))
selected.setItem(
4, k,
QTableWidgetItem(
data.status[i].values[0].value))
except IndexError:
rospy.logwarn(
"Received a message that misses some information. Faulty message: "
+ data.status[i])
self._widget.tableWidget.setHorizontalHeaderLabels(
self.motorheaderlist)
self._widget.tableWidget_2.setHorizontalHeaderLabels(
self.motorheaderlist[10:])
self._widget.tableWidget.reset()
self._widget.tableWidget_2.reset()
self._widget.tableWidget.setEditTriggers(
QTableWidget.NoEditTriggers)
self.tetrigger.emit()
self.votrigger.emit()
def set_motor_joint_states(self, data):
if self.current_tab == 0:
self.motorheaderlist = []
for i in range(0, len(data.name)):
self.torquelist[i].append(float(data.effort[i]))
if not data.name[i] in self.motorheaderlist:
self.motorheaderlist.append(data.name[i])
for j in range(0, len(self.motorheaderlist)):
if j < 10:
selected = self._widget.tableWidget
k = j
else:
selected = self._widget.tableWidget_2
k = j - 10
for j in self.motorheaderlist:
if j == data.name[i]:
selected.setItem(
2, k,
QTableWidgetItem(
str(
round(
float(
math.degrees(
data.position[i])), 2))))
selected.setItem(
5, k,
QTableWidgetItem(
str(round(float(data.effort[i]), 2))))
self._widget.tableWidget.setHorizontalHeaderLabels(
self.motorheaderlist)
self._widget.tableWidget_2.setHorizontalHeaderLabels(
self.motorheaderlist[10:])
self.totrigger.emit()
def update_graph(self, graph, glist, cbox, number):
"""Updates the graph that displays the motors temperature"""
if self.current_tab == number:
graph.clear()
graph.setXRange(-1, 21)
for i in range(len(glist)):
if cbox.currentText() == 'All' or cbox.currentIndex() - 1 == i:
path = pg.arrayToQPath(np.arange(len(glist[i])), glist[i])
item = QtGui.QGraphicsPathItem(path)
item.setPen(pg.mkPen(color=self.colorlist[i]))
graph.addItem(item)
def make_Graphs(self):
"""Method to initialize the different plots.
Creates a plot with grid and legend, then adds it to the coresponding layout.
"""
self.tempplt = pg.plot()
self.tempplt.setYRange(-1, 100)
self.tempplt.showGrid(x=True, y=True)
self.tempplt.addLegend()
for i in range(0, 10):
self.tempplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.tempplt.addLegend(offset=79)
#creates a legend by plotting a single point for each motor, just so they show up in the legend.
#Apparently you are supposed to do it that way...
for i in range(10, 20):
self.tempplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.layout_temp = QtGui.QHBoxLayout()
self.combobox3 = QComboBox()
self.combobox3.addItem('All')
for i in self.motornames:
self.combobox3.addItem(i)
self._widget.Temperatur.setLayout(self.layout_temp)
self.layout_temp.addWidget(self.tempplt)
self.layout_temp.addWidget(self.combobox3)
self.tempplt.win.hide()
self.torqueplt = pg.plot()
self.torqueplt.setYRange(-1, 2)
self.torqueplt.showGrid(x=True, y=True)
self.torqueplt.addLegend()
for i in range(0, 10):
self.torqueplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.torqueplt.addLegend(offset=79)
for i in range(10, 20):
self.torqueplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.layout_torque = QtGui.QHBoxLayout()
self._widget.Torque.setLayout(self.layout_torque)
self.layout_torque.addWidget(self.torqueplt)
self.combobox = QComboBox()
self.combobox.addItem('All')
for i in self.motornames:
self.combobox.addItem(i)
self.layout_torque.addWidget(self.combobox)
self.torqueplt.win.hide()
self.voltageplt = pg.plot()
self.voltageplt.setYRange(-1, 30)
self.voltageplt.showGrid(x=True, y=True)
self.voltageplt.addLegend()
for i in range(0, 10):
self.voltageplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.voltageplt.addLegend(offset=79)
for i in range(10, 20):
self.voltageplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.layout_voltage = QtGui.QHBoxLayout()
self.combobox2 = QComboBox()
self.combobox2.addItem('All')
for i in self.motornames:
self.combobox2.addItem(i)
self._widget.Voltage.setLayout(self.layout_voltage)
self.layout_voltage.addWidget(self.voltageplt)
self.layout_voltage.addWidget(self.combobox2)
self.voltageplt.win.hide()
def __init__(self, context):
super(RqtHandeyeCalibration, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('HandeyeCalibration')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print('arguments: ', args)
print('unknowns: ', unknowns)
# Create QWidgets
self._widget = QWidget()
self._infoWidget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_easy_handeye'),
'resource', 'rqt_handeye.ui')
ui_info_file = os.path.join(
rospkg.RosPack().get_path('rqt_easy_handeye'), 'resource',
'rqt_handeye_info.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
loadUi(ui_info_file, self._infoWidget)
self._widget.horizontalLayout_infoAndActions.insertWidget(
0, self._infoWidget)
self._calibration_algorithm_combobox = QComboBox()
self._calibration_algorithm_layout = QHBoxLayout()
self._calibration_algorithm_layout.insertWidget(
0, QLabel('Calibration algorithm: '))
self._calibration_algorithm_layout.insertWidget(
1, self._calibration_algorithm_combobox)
self._infoWidget.layout().insertLayout(
-1, self._calibration_algorithm_layout)
# Give QObjects reasonable names
self._widget.setObjectName('RqtHandeyeCalibrationUI')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.client = HandeyeClient()
resp = self.client.list_algorithms()
for i, a in enumerate(resp.algorithms):
self._calibration_algorithm_combobox.insertItem(i, a)
index_of_curr_alg = resp.algorithms.index(resp.current_algorithm)
self._calibration_algorithm_combobox.setCurrentIndex(index_of_curr_alg)
self._calibration_algorithm_combobox.currentTextChanged.connect(
self.client.set_algorithm)
self._infoWidget.calibNameLineEdit.setText(rospy.get_namespace())
self._infoWidget.trackingBaseFrameLineEdit.setText(
self.client.parameters.tracking_base_frame)
self._infoWidget.trackingMarkerFrameLineEdit.setText(
self.client.parameters.tracking_marker_frame)
self._infoWidget.robotBaseFrameLineEdit.setText(
self.client.parameters.robot_base_frame)
self._infoWidget.robotEffectorFrameLineEdit.setText(
self.client.parameters.robot_effector_frame)
if self.client.parameters.eye_on_hand:
self._infoWidget.calibTypeLineEdit.setText("eye on hand")
else:
self._infoWidget.calibTypeLineEdit.setText("eye on base")
self._widget.takeButton.clicked[bool].connect(self.handle_take_sample)
self._widget.removeButton.clicked[bool].connect(
self.handle_remove_sample)
self._widget.computeButton.clicked[bool].connect(
self.handle_compute_calibration)
self._widget.saveButton.clicked[bool].connect(
self.handle_save_calibration)
self._widget.removeButton.setEnabled(False)
self._widget.computeButton.setEnabled(False)
self._widget.saveButton.setEnabled(False)
def make_Graphs(self):
"""Method to initialize the different plots.
Creates a plot with grid and legend, then adds it to the coresponding layout.
"""
self.tempplt = pg.plot()
self.tempplt.setYRange(-1, 100)
self.tempplt.showGrid(x=True, y=True)
self.tempplt.addLegend()
for i in range(0, 10):
self.tempplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.tempplt.addLegend(offset=79)
#creates a legend by plotting a single point for each motor, just so they show up in the legend.
#Apparently you are supposed to do it that way...
for i in range(10, 20):
self.tempplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.layout_temp = QtGui.QHBoxLayout()
self.combobox3 = QComboBox()
self.combobox3.addItem('All')
for i in self.motornames:
self.combobox3.addItem(i)
self._widget.Temperatur.setLayout(self.layout_temp)
self.layout_temp.addWidget(self.tempplt)
self.layout_temp.addWidget(self.combobox3)
self.tempplt.win.hide()
self.torqueplt = pg.plot()
self.torqueplt.setYRange(-1, 2)
self.torqueplt.showGrid(x=True, y=True)
self.torqueplt.addLegend()
for i in range(0, 10):
self.torqueplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.torqueplt.addLegend(offset=79)
for i in range(10, 20):
self.torqueplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.layout_torque = QtGui.QHBoxLayout()
self._widget.Torque.setLayout(self.layout_torque)
self.layout_torque.addWidget(self.torqueplt)
self.combobox = QComboBox()
self.combobox.addItem('All')
for i in self.motornames:
self.combobox.addItem(i)
self.layout_torque.addWidget(self.combobox)
self.torqueplt.win.hide()
self.voltageplt = pg.plot()
self.voltageplt.setYRange(-1, 30)
self.voltageplt.showGrid(x=True, y=True)
self.voltageplt.addLegend()
for i in range(0, 10):
self.voltageplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.voltageplt.addLegend(offset=79)
for i in range(10, 20):
self.voltageplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.layout_voltage = QtGui.QHBoxLayout()
self.combobox2 = QComboBox()
self.combobox2.addItem('All')
for i in self.motornames:
self.combobox2.addItem(i)
self._widget.Voltage.setLayout(self.layout_voltage)
self.layout_voltage.addWidget(self.voltageplt)
self.layout_voltage.addWidget(self.combobox2)
self.voltageplt.win.hide()