def __init__(self, status, close_callback):
"""
:type status: DiagnosticStatus
:param close_callback: When the instance of this class
(InspectorWindow) terminates, this callback gets
called.
"""
#TODO(Isaac) UI construction that currently is done in this method,
# needs to be done in .ui file.
super(InspectorWindow, self).__init__()
self.status = status
self._close_callback = close_callback
self.setWindowTitle(status.name)
self.paused = False
self.layout_vertical = QVBoxLayout(self)
self.disp = QTextEdit(self)
self.snapshot = QPushButton("StatusSnapshot")
self.timeline_pane = TimelinePane(self)
self.timeline_pane.set_timeline_data(Util.SECONDS_TIMELINE,
self.get_color_for_value,
self.on_pause)
self.layout_vertical.addWidget(self.disp, 1)
self.layout_vertical.addWidget(self.timeline_pane, 0)
self.layout_vertical.addWidget(self.snapshot)
self.snaps = []
self.snapshot.clicked.connect(self._take_snapshot)
self._sig_write.connect(self._write_key_val)
self._sig_newline.connect(lambda: self.disp.insertPlainText('\n'))
self._sig_clear.connect(lambda: self.disp.clear())
self._sig_close_window.connect(self._close_callback)
self.setLayout(self.layout_vertical)
# TODO better to be configurable where to appear.
self.setGeometry(0, 0, 400, 600)
self.show()
self.update_status_display(status)
def createEditor(self, parent, option, index):
editor = QTextEdit(parent)
highlight = syntax.PythonHighlighter(editor.document())
font = QFont("Courier")
font.setFamily("Courier");
font.setStyleHint(QFont.Monospace);
font.setFixedPitch(True);
font.setPointSize(10);
editor.setFont(font)
tab_stop = 4; # 4 characters
metrics = QFontMetrics(font)
editor.setTabStopWidth(tab_stop * metrics.width(' '));
return editor
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 JointStatePublisherGui(QWidget):
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()
def who_data_changed(self):
for i in range(0, self.num_rows):
value_last = self.value_line_edit[i].text()
value_last = float(value_last) * 1000
value = self.sliders[i].value()
if value != value_last:
return i
def change_line_edit(self, change_index):
value = self.sliders[change_index].value()
value = float(value) / 1000
value = str(value)
self.value_line_edit[change_index].setText(value)
def change_position_edit(self, change_index):
value = self.progressbars[change_index].value()
value += self.sliders[change_index].value()
self.progressbars[change_index].setValue(value)
self.positions[change_index].setText(str(float(value) / 1000))
def reset_speed(self, change_index):
self.sliders[change_index].setValue(0)
def reset_speed_all(self):
for i in range(0, self.num_rows):
self.reset_speed(i)
def set_speed(self, index, data):
self.sliders[index].setValue(data)
def import_Btn_clecked(self):
self.file_path = self.f.getOpenFileName(caption='Import excel data',
directory='',
filter='*.xlsx',
initialFilter='')
self.file_path = self.file_path[0]
if self.callback_import:
self.callback_import()
pass
def export_Btn_clicked(self):
self.file_path = self.f.getSaveFileName(caption='Save as excel data',
directory='',
filter='*.xlsx',
initialFilter='')
self.file_path = self.file_path[0]
if self.callback_export:
self.callback_export()
pass
def set_callback_start(self, func):
self.callback_start = func
def start_Btn_clicked(self):
if self.start_Btn.text() == "Start":
if self.callback_start:
self.callback_start()
self.start_Btn.setText("Pause")
else:
if self.callback_pause:
self.callback_pause()
self.start_Btn.setText("Start")
def reset_Btn_clicked(self):
self.reset_speed_all()
self.reset_postion_all()
if self.callback_reset:
self.callback_reset()
def replay_Btn_clicked(self):
if self.replay_Btn.text() == "Replay":
self.replay_Btn.setText("Stop")
if self.callback_replay:
self.callback_replay()
else:
self.replay_Btn.setText("Replay")
if self.callback_replay_stop:
self.callback_replay_stop()
def debug_Btn_clicked(self):
if self.callback_debug:
self.callback_debug()
def record_Btn_clicked(self):
self.set_postion()
self.listVeiw_len += 1
if self.callback_record:
self.callback_record()
self.reset_speed_all()
def listVeiw_itemSelectionChanged(self, index):
self.listVeiw_isClicked = True
self.listVeiw_current_item = index
if self.callback_list_clicked:
self.callback_list_clicked()
def listVeiw_clicked(self, index):
#print "index", index.row()
if self.listVeiw_current_item != index.row():
description_text = self.description.toPlainText()
#self.listVeiw.item(self.listVeiw_current_item).setData(1,"123")
#print self.listVeiw.item(self.listVeiw_current_item).data
self.listVeiw_isClicked = True
self.listVeiw_current_item = index.row()
if self.callback_list_clicked:
self.callback_list_clicked()
def update_listView(self):
print "update", self.listVeiw_len
for i in range(0, self.listVeiw_len):
view = self.listVeiw.item(i)
view.setText(str(i))
def get_listVeiw_current_item(self):
return self.listVeiw_current_item
def delete_Btn_clicked(self):
if self.listVeiw_isClicked:
print self.listVeiw_current_item
self.listVeiw.removeItemWidget(
self.listVeiw.takeItem(self.listVeiw_current_item))
self.listVeiw_len -= 1
if self.listVeiw_current_item != self.listVeiw_len:
self.update_listView()
if self.callback_delete:
self.callback_delete()
if self.listVeiw_current_item == 0:
self.listVeiw_current_item = 0
else:
self.listVeiw_current_item -= 1
self.listVeiw.setCurrentRow(self.listVeiw_current_item)
if self.listVeiw_len == 0:
self.listVeiw_isClicked = False
def listView_add_item(self, index):
self.listVeiw.addItem(str(index))
def listView_inset_item(self, index, label):
self.listVeiw.insertItem(index, str(label))
def slider_value_changed(self, data):
change_index = self.who_data_changed()
self.change_line_edit(change_index)
def get_speed(self):
speed = []
for i in range(0, self.num_rows):
value = self.sliders[i].value()
value = float(value) / 1000
speed.append(value)
return speed
def get_position(self):
position = []
for i in range(0, self.num_rows):
value = self.progressbars[i].value()
value += self.sliders[i].value()
value = float(value) / 1000
position.append(value)
return position
def set_postion(self):
for i in range(0, self.num_rows):
self.change_position_edit(i)
def set_positions(self, data):
print len(data)
for i in range(0, len(data)):
self.progressbars[i].setValue(data[i] * 1000)
self.positions[i].setText(str(data[i]))
def reset_position(self, change_index):
value = 0
self.progressbars[change_index].setValue(value)
self.positions[change_index].setText(str(float(value) / 1000))
def reset_postion_all(self):
for i in range(0, self.num_rows):
self.reset_position(i)
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
def __init__(self, context):
"""
Create Qt GUI using the event file
"""
super(EventTransmissionGUI, self).__init__(context)
self.setObjectName('ManualEventTriggerGUI')
parser = ArgumentParser()
# Add argument(s) to the parser.
args = self._parse_args(context.argv())
## Create Event trigger
self.event_trigger = RosEventTrigger(args.event_file)
## Parent container to store buttons, textboxes
self._container = QTabWidget()
## Tab to display robot system, state machine status
## and command buttons without any arguments
self._status_tab = QWidget()
## Tab to display positionyaw and velocityyaw commands
## and sliders for them
self._additional_commands_tab = QWidget()
# Set title of the parent container window
self._status_tab.setWindowTitle(self.event_trigger.event_manager_name)
## Layout for status tab
self._layout = QVBoxLayout()
self._status_tab.setLayout(self._layout)
# Create Textboxes and add to Layout
self._layout.addWidget(QLabel('State Machine State'))
## Textbox to show sytem status
self.system_status_textbox = QTextEdit()
self.system_status_textbox.setReadOnly(True)
self._layout.addWidget(self.system_status_textbox)
# Define and connect buttons
self._layout.addWidget(QLabel('Event Triggers'))
## Continer to store event triggering buttons
self.button_container = QWidget()
## List of push buttons to trigger events
self.push_buttons = list()
## Layout for the push buttons
self.button_layout = QGridLayout()
self.button_container.setLayout(self.button_layout)
button_index = 0
for event_name in self.event_trigger.event_names_list:
self.push_buttons.append(QPushButton(event_name))
partial_fun = partial(self.event_trigger.triggerEvent,
event_name=event_name)
self.push_buttons[-1].clicked.connect(partial_fun)
row, col = self.get_row_col(button_index, args.grid_cols)
self.button_layout.addWidget(self.push_buttons[-1], row, col)
button_index += 1
self._layout.addWidget(self.button_container)
## Layout for additional commands tab
self._additional_commands_layout = QVBoxLayout()
self._additional_commands_tab.setLayout(
self._additional_commands_layout)
# Create height slider
self._additional_commands_layout.addWidget(
QLabel('Pose Command Height (m)'))
## Height slider to adjust z coordinate for pose command
## \todo Matt: Load slider settings from param file
self.height_slider = self.createSlider(1.0, 20.0, 2.0, 1.0)
self._additional_commands_layout.addWidget(self.height_slider)
## Add button for triggering pose command
## Container for pose event related objects: slide etc
## \todo Matt: Reset slider value based on current quad height
self.pose_command_container = QWidget()
## Pose command layout
self.pose_command_layout = QGridLayout()
self.pose_command_container.setLayout(self.pose_command_layout)
## x pose label to display position command from rviz to user
self.pose_x = QLabel('x: -')
## y pose label to display position command from rviz to user
self.pose_y = QLabel('y: -')
## z pose label to display position command from rviz to user
self.pose_z = QLabel("z: {0:.2f}".format(self.height_slider.value()))
self.height_slider.valueChanged.connect(
partial(self.updateLabel,
header="z",
label=self.pose_z,
slider=self.height_slider))
self.pose_command_layout.addWidget(self.pose_x, 0, 0)
self.pose_command_layout.addWidget(self.pose_y, 0, 1)
self.pose_command_layout.addWidget(self.pose_z, 0, 2)
## Button to send the pose command to state machine as poseyaw event
self.send_pose_command_button = QPushButton("Send Pose Command")
self.send_pose_command_button.clicked.connect(
self.poseCommandButtonCallback)
self.pose_command_layout.addWidget(self.send_pose_command_button, 0, 3)
self._additional_commands_layout.addWidget(self.pose_command_container)
## Pose command container to store pose from Rviz and send to state
## machine
self.pose_command = None
## Sliders for setting vx,vy,vz, yaw
self.velocity_sliders = []
## Labels vx,vy,vz,yaw as an array
self.velocity_command_labels = []
self._additional_commands_layout.addWidget(
QLabel('Velocity Command (m/s), Yaw (deg)'))
for i in range(3):
self.velocity_sliders.append(
self.createSlider(-20.0, 20.0, 0.0, 1.0))
self._additional_commands_layout.addWidget(
self.velocity_sliders[i])
# Slider for yaw
self.velocity_sliders.append(
self.createSlider(-180.0, 180.0, 0.0, 10.0))
self._additional_commands_layout.addWidget(self.velocity_sliders[-1])
## Add button for triggering velocity yaw
self.velocity_command_container = QWidget()
## Velocity command layout
self.velocity_command_layout = QGridLayout()
self.velocity_command_container.setLayout(self.velocity_command_layout)
for i, axis_label in enumerate(['x', 'y', 'z']):
# label to display velocity command from rviz to user
self.velocity_command_labels.append(
QLabel("v{0}: {1:.2f}".format(
axis_label, self.velocity_sliders[i].value() / 10.0)))
self.velocity_sliders[i].valueChanged.connect(
partial(self.updateLabel,
header="v" + str(i),
label=self.velocity_command_labels[i],
slider=self.velocity_sliders[i],
scale=10.0))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[i], 0, i)
# Label for yaw
self.velocity_command_labels.append(
QLabel("Yaw: {0:.2f}".format(self.velocity_sliders[-1].value())))
self.velocity_sliders[-1].valueChanged.connect(
partial(self.updateLabel,
header="Yaw",
label=self.velocity_command_labels[-1],
slider=self.velocity_sliders[-1]))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[-1], 0, 3)
## Button to send the pose command to state machine as poseyaw event
self.send_velocity_command_button = QPushButton(
"Send Velocity Command")
self.send_velocity_command_button.clicked.connect(
self.velocityCommandButtonCallback)
self.velocity_command_layout.addWidget(
self.send_velocity_command_button, 0, 4)
self._additional_commands_layout.addWidget(
self.velocity_command_container)
self._additional_commands_layout.addStretch()
self._container.addTab(self._status_tab, "StatusBasicCommands")
self._container.addTab(self._additional_commands_tab,
"AdditionalCommands")
context.add_widget(self._container)
# Add textboxes to update hooks from eventTrigger class
# Define Partial callbacks
systemStatusCallback = partial(self.updateStatus,
text_box=self.system_status_textbox)
# Connect Event Triggers
self.event_trigger.status_signal.connect(systemStatusCallback)
self.event_trigger.pose_command_signal.connect(
self.poseCommandCallback)
class EventTransmissionGUI(Plugin):
"""
GUI to send events from User to logic state machine
"""
def __init__(self, context):
"""
Create Qt GUI using the event file
"""
super(EventTransmissionGUI, self).__init__(context)
self.setObjectName('ManualEventTriggerGUI')
parser = ArgumentParser()
# Add argument(s) to the parser.
args = self._parse_args(context.argv())
## Create Event trigger
self.event_trigger = RosEventTrigger(args.event_file)
## Parent container to store buttons, textboxes
self._container = QTabWidget()
## Tab to display robot system, state machine status
## and command buttons without any arguments
self._status_tab = QWidget()
## Tab to display positionyaw and velocityyaw commands
## and sliders for them
self._additional_commands_tab = QWidget()
# Set title of the parent container window
self._status_tab.setWindowTitle(self.event_trigger.event_manager_name)
## Layout for status tab
self._layout = QVBoxLayout()
self._status_tab.setLayout(self._layout)
# Create Textboxes and add to Layout
self._layout.addWidget(QLabel('State Machine State'))
## Textbox to show sytem status
self.system_status_textbox = QTextEdit()
self.system_status_textbox.setReadOnly(True)
self._layout.addWidget(self.system_status_textbox)
# Define and connect buttons
self._layout.addWidget(QLabel('Event Triggers'))
## Continer to store event triggering buttons
self.button_container = QWidget()
## List of push buttons to trigger events
self.push_buttons = list()
## Layout for the push buttons
self.button_layout = QGridLayout()
self.button_container.setLayout(self.button_layout)
button_index = 0
for event_name in self.event_trigger.event_names_list:
self.push_buttons.append(QPushButton(event_name))
partial_fun = partial(self.event_trigger.triggerEvent,
event_name=event_name)
self.push_buttons[-1].clicked.connect(partial_fun)
row, col = self.get_row_col(button_index, args.grid_cols)
self.button_layout.addWidget(self.push_buttons[-1], row, col)
button_index += 1
self._layout.addWidget(self.button_container)
## Layout for additional commands tab
self._additional_commands_layout = QVBoxLayout()
self._additional_commands_tab.setLayout(
self._additional_commands_layout)
# Create height slider
self._additional_commands_layout.addWidget(
QLabel('Pose Command Height (m)'))
## Height slider to adjust z coordinate for pose command
## \todo Matt: Load slider settings from param file
self.height_slider = self.createSlider(1.0, 20.0, 2.0, 1.0)
self._additional_commands_layout.addWidget(self.height_slider)
## Add button for triggering pose command
## Container for pose event related objects: slide etc
## \todo Matt: Reset slider value based on current quad height
self.pose_command_container = QWidget()
## Pose command layout
self.pose_command_layout = QGridLayout()
self.pose_command_container.setLayout(self.pose_command_layout)
## x pose label to display position command from rviz to user
self.pose_x = QLabel('x: -')
## y pose label to display position command from rviz to user
self.pose_y = QLabel('y: -')
## z pose label to display position command from rviz to user
self.pose_z = QLabel("z: {0:.2f}".format(self.height_slider.value()))
self.height_slider.valueChanged.connect(
partial(self.updateLabel,
header="z",
label=self.pose_z,
slider=self.height_slider))
self.pose_command_layout.addWidget(self.pose_x, 0, 0)
self.pose_command_layout.addWidget(self.pose_y, 0, 1)
self.pose_command_layout.addWidget(self.pose_z, 0, 2)
## Button to send the pose command to state machine as poseyaw event
self.send_pose_command_button = QPushButton("Send Pose Command")
self.send_pose_command_button.clicked.connect(
self.poseCommandButtonCallback)
self.pose_command_layout.addWidget(self.send_pose_command_button, 0, 3)
self._additional_commands_layout.addWidget(self.pose_command_container)
## Pose command container to store pose from Rviz and send to state
## machine
self.pose_command = None
## Sliders for setting vx,vy,vz, yaw
self.velocity_sliders = []
## Labels vx,vy,vz,yaw as an array
self.velocity_command_labels = []
self._additional_commands_layout.addWidget(
QLabel('Velocity Command (m/s), Yaw (deg)'))
for i in range(3):
self.velocity_sliders.append(
self.createSlider(-20.0, 20.0, 0.0, 1.0))
self._additional_commands_layout.addWidget(
self.velocity_sliders[i])
# Slider for yaw
self.velocity_sliders.append(
self.createSlider(-180.0, 180.0, 0.0, 10.0))
self._additional_commands_layout.addWidget(self.velocity_sliders[-1])
## Add button for triggering velocity yaw
self.velocity_command_container = QWidget()
## Velocity command layout
self.velocity_command_layout = QGridLayout()
self.velocity_command_container.setLayout(self.velocity_command_layout)
for i, axis_label in enumerate(['x', 'y', 'z']):
# label to display velocity command from rviz to user
self.velocity_command_labels.append(
QLabel("v{0}: {1:.2f}".format(
axis_label, self.velocity_sliders[i].value() / 10.0)))
self.velocity_sliders[i].valueChanged.connect(
partial(self.updateLabel,
header="v" + str(i),
label=self.velocity_command_labels[i],
slider=self.velocity_sliders[i],
scale=10.0))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[i], 0, i)
# Label for yaw
self.velocity_command_labels.append(
QLabel("Yaw: {0:.2f}".format(self.velocity_sliders[-1].value())))
self.velocity_sliders[-1].valueChanged.connect(
partial(self.updateLabel,
header="Yaw",
label=self.velocity_command_labels[-1],
slider=self.velocity_sliders[-1]))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[-1], 0, 3)
## Button to send the pose command to state machine as poseyaw event
self.send_velocity_command_button = QPushButton(
"Send Velocity Command")
self.send_velocity_command_button.clicked.connect(
self.velocityCommandButtonCallback)
self.velocity_command_layout.addWidget(
self.send_velocity_command_button, 0, 4)
self._additional_commands_layout.addWidget(
self.velocity_command_container)
self._additional_commands_layout.addStretch()
self._container.addTab(self._status_tab, "StatusBasicCommands")
self._container.addTab(self._additional_commands_tab,
"AdditionalCommands")
context.add_widget(self._container)
# Add textboxes to update hooks from eventTrigger class
# Define Partial callbacks
systemStatusCallback = partial(self.updateStatus,
text_box=self.system_status_textbox)
# Connect Event Triggers
self.event_trigger.status_signal.connect(systemStatusCallback)
self.event_trigger.pose_command_signal.connect(
self.poseCommandCallback)
def createSlider(self, minimum, maximum, default_value, tick_interval):
"""
Create a QtSlider with specified properties
Parameters:
@param minimum Minimum value for slider
@param maximum Maximum value for slider
@param default_value Initial value the slider is set to
@param tick_inverval Integer value specifying difference between
successive ticks
@return Slider value
"""
slider = QSlider(Qt.Horizontal)
slider.setMinimum(minimum)
slider.setMaximum(maximum)
slider.setValue(default_value)
slider.setTickPosition(QSlider.TicksBelow)
slider.setTickInterval(tick_interval)
return slider
def _parse_args(self, argv):
"""
Parse extra arguments when plugin is deployed in standalone mode
"""
parser = argparse.ArgumentParser(prog='aerial_autonomy',
add_help=False)
EventTransmissionGUI.add_arguments(parser)
return parser.parse_args(argv)
@staticmethod
def add_arguments(parser):
"""
Notify rqt_gui that this plugin can parse these extra arguments
"""
group = parser.add_argument_group(
'Options for aerial autonomy gui plugin')
group.add_argument("-e",
"--event_file",
type=str,
default='',
help="Event file")
group.add_argument("-c",
"--grid_cols",
type=int,
default=3,
help="Number of columns in grid")
def get_row_col(self, button_index, ncols):
"""
Automatically find the row and col to add the button
to in a grid based on index of the button
"""
col_index = button_index % ncols
row_index = int((button_index - col_index) / ncols)
return (row_index, col_index)
def poseCommandCallback(self, pose):
"""
Saves pose command and updates command display
"""
self.pose_command = pose
self.pose_x.setText("x: {0:.2f}".format(
self.pose_command.pose.position.x))
self.pose_y.setText("y: {0:.2f}".format(
self.pose_command.pose.position.y))
def poseCommandButtonCallback(self):
"""
Publishes stored pose command after setting height from slider
"""
if self.pose_command:
self.pose_command.pose.position.z = self.height_slider.value()
self.event_trigger.triggerPoseCommand(self.pose_command)
# Reset pose command to avoid accidental triggering
self.pose_command = None
self.pose_x.setText('x: -')
self.pose_y.setText('y: -')
else:
print "No pose command to trigger"
def velocityCommandButtonCallback(self):
"""
Publishes stored velocity command
"""
velocity_command = VelocityYaw()
velocity_command.vx = self.velocity_sliders[0].value() / 10.0
velocity_command.vy = self.velocity_sliders[1].value() / 10.0
velocity_command.vz = self.velocity_sliders[2].value() / 10.0
velocity_command.yaw = self.velocity_sliders[3].value() * np.pi / 180.0
self.event_trigger.triggerVelocityCommand(velocity_command)
def updateLabel(self, header, label, slider, scale=1):
"""
Updates height label based on slider value
"""
label.setText(header + ": {0:.2f}".format(slider.value() / scale))
def updateStatus(self, status, text_box):
"""
Generic placeholder function to update text box
"""
if not sip.isdeleted(text_box):
text_box.setHtml(status)
doc_size = text_box.document().size()
text_box.setFixedHeight(doc_size.height() + 10)
class InspectorWindow(AbstractStatusWidget):
_sig_write = Signal(str, str)
_sig_newline = Signal()
_sig_close_window = Signal()
_sig_clear = Signal()
def __init__(self, status, close_callback):
"""
:type status: DiagnosticStatus
:param close_callback: When the instance of this class
(InspectorWindow) terminates, this callback gets
called.
"""
#TODO(Isaac) UI construction that currently is done in this method,
# needs to be done in .ui file.
super(InspectorWindow, self).__init__()
self.status = status
self._close_callback = close_callback
self.setWindowTitle(status.name)
self.paused = False
self.layout_vertical = QVBoxLayout(self)
self.disp = QTextEdit(self)
self.snapshot = QPushButton("StatusSnapshot")
self.timeline_pane = TimelinePane(self)
self.timeline_pane.set_timeline_data(Util.SECONDS_TIMELINE,
self.get_color_for_value,
self.on_pause)
self.layout_vertical.addWidget(self.disp, 1)
self.layout_vertical.addWidget(self.timeline_pane, 0)
self.layout_vertical.addWidget(self.snapshot)
self.snaps = []
self.snapshot.clicked.connect(self._take_snapshot)
self._sig_write.connect(self._write_key_val)
self._sig_newline.connect(lambda: self.disp.insertPlainText('\n'))
self._sig_clear.connect(lambda: self.disp.clear())
self._sig_close_window.connect(self._close_callback)
self.setLayout(self.layout_vertical)
# TODO better to be configurable where to appear.
self.setGeometry(0, 0, 400, 600)
self.show()
self.update_status_display(status)
def closeEvent(self, event):
# emit signal that should be slotted by StatusItem
self._sig_close_window.emit()
self.close()
def _write_key_val(self, k, v):
self.disp.setFontWeight(75)
self.disp.insertPlainText(k)
self.disp.insertPlainText(': ')
self.disp.setFontWeight(50)
self.disp.insertPlainText(v)
self.disp.insertPlainText('\n')
def pause(self, msg):
rospy.logdebug('InspectorWin pause PAUSED')
self.paused = True
self.update_status_display(msg)
def unpause(self, msg):
rospy.logdebug('InspectorWin pause UN-PAUSED')
self.paused = False
def new_diagnostic(self, msg, is_forced=False):
"""
Overridden from AbstractStatusWidget
:type status: DiagnosticsStatus
"""
if not self.paused:
self.update_status_display(msg)
rospy.logdebug('InspectorWin _cb len of queue=%d self.paused=%s',
len(self.timeline_pane._queue_diagnostic),
self.paused)
else:
if is_forced:
self.update_status_display(msg, True)
rospy.logdebug('@@@InspectorWin _cb PAUSED window updated')
else:
rospy.logdebug('@@@InspectorWin _cb PAUSED not updated')
def update_status_display(self, status, is_forced=False):
"""
:type status: DiagnosticsStatus
"""
if not self.paused or (self.paused and is_forced):
scroll_value = self.disp.verticalScrollBar().value()
self.timeline_pane.new_diagnostic(status)
rospy.logdebug('InspectorWin update_status_display 1')
self.status = status
self._sig_clear.emit()
self._sig_write.emit("Full Name", status.name)
self._sig_write.emit("Component", status.name.split('/')[-1])
self._sig_write.emit("Hardware ID", status.hardware_id)
self._sig_write.emit("Level", str(status.level))
self._sig_write.emit("Message", status.message)
self._sig_newline.emit()
for v in status.values:
self._sig_write.emit(v.key, v.value)
if self.disp.verticalScrollBar().maximum() < scroll_value:
scroll_value = self.disp.verticalScrollBar().maximum()
self.disp.verticalScrollBar().setValue(scroll_value)
def _take_snapshot(self):
snap = StatusSnapshot(self.status)
self.snaps.append(snap)
def get_color_for_value(self, queue_diagnostic, color_index):
"""
Overridden from AbstractStatusWidget.
:type color_index: int
"""
rospy.logdebug(
'InspectorWindow get_color_for_value ' +
'queue_diagnostic=%d, color_index=%d', len(queue_diagnostic),
color_index)
lv_index = queue_diagnostic[color_index - 1].level
return Util.COLOR_DICT[lv_index]