def motor_controller(self):
'''
Sets up the GUI in the middle of the Screen to control the motors.
Uses self._motorValues to determine which motors are present.
'''
i = 0
for k, v in sorted(self._currentGoals.items()):
group = QGroupBox()
slider = QSlider(Qt.Horizontal)
slider.setTickInterval(1)
slider.setMinimum(-181)
slider.setMaximum(181)
slider.valueChanged.connect(self.slider_update)
self._sliders[k] = slider
textfield = QLineEdit()
textfield.setText('0')
textfield.textEdited.connect(self.textfield_update)
self._textFields[k] = textfield
label = QLabel()
label.setText(k)
layout = QVBoxLayout()
layout.addWidget(label)
layout.addWidget(self._sliders[k])
layout.addWidget(self._textFields[k])
group.setLayout(layout)
self._widget.motorControlLayout.addWidget(group, i / 5, i % 5)
i = i+1
class QualityDialog(QDialog):
def __init__(self, bagFiles, parent=None):
super(QualityDialog, self).__init__()
self.parent = parent
self.bagFiles = bagFiles
self.setWindowTitle("Data Quality")
## Widgets:
self.spinBox = QDoubleSpinBox()
self.spinBox.setDecimals(2)
self.spinBox.setMaximum(1.0)
self.spinBox.setSingleStep(0.1)
self.spinBox.setValue(0.5)
self.fppiLbl = QLabel("FPPI = ")
self.motaLbl = QLabel("MOTA = ")
self.motpLbl = QLabel("MOTP = ")
self.recalcBtn = QPushButton("Recalculate Quality")
self.recalcBtn.clicked.connect(self.calculate)
## Layout:
layout = QVBoxLayout()
layout.addWidget(self.spinBox)
layout.addWidget(self.fppiLbl)
layout.addWidget(self.motaLbl)
layout.addWidget(self.motpLbl)
layout.addWidget(self.recalcBtn)
self.setLayout(layout)
self.resize(300, 200)
# calculate the data quality:
self.calculate()
def calculate(self):
if self.bagFiles[0] == "" or self.bagFiles[1] == "":
message_module.showMessage(
"Bag file missing! Please import bag file in the main interface."
)
return
threshold = self.spinBox.value()
fppi = Rosbag_Analysis.getFPPI(self.bagFiles[0], self.bagFiles[1],
threshold)
self.fppiLbl.setText("FPPI = " + str(fppi))
mota = Rosbag_Analysis.getMOTA(self.bagFiles[0], self.bagFiles[1],
threshold)
self.motaLbl.setText("MOTA = " + str(mota))
motp = Rosbag_Analysis.getMOTP(self.bagFiles[0], self.bagFiles[1],
threshold)
self.motpLbl.setText("MOTP = " + str(motp))
class HistorySelection(QWidget):
def __init__(self):
super(HistorySelection, self).__init__()
self.setWindowTitle("Extracting Window Features")
self.resize(550, 440)
per_title, pre = TS.get_time_series_pre_feature_options()
glob_title, glob = TS.get_global_time_series_features_options()
self.history_items = dict()
self.history_items[per_title] = pre
self.history_items[glob_title] = glob
self.saved_dir = []
self.group_selected_items = dict()
self.group_areas = dict()
self.group_main_widget = dict()
self.group_selection_vlayout = dict()
self.group_item_all = dict()
self.main_vlayout = QVBoxLayout(self)
self.group_label = dict()
self.items_list = []
self.selected_topics = []
self.files = []
# layout = QFormLayout()
self.select_path = QLineEdit()
self.save_path = QLineEdit()
self.select_path.setEnabled(False)
self.save_path.setEnabled(False)
self.ok_button = QPushButton("Select CSV...", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.two_buttons1 = QHBoxLayout(self)
self.two_buttons1.addWidget(self.ok_button)
self.two_buttons1.addWidget(self.select_path)
self.main_vlayout.addLayout(self.two_buttons1)
# self.main_vlayout.addRow(self.ok_button, self.client_answers)
# self.main_vlayout.addWidget(self.ok_button)
# self.main_vlayout.addWidget(self.ok_button)
for group_name in self.history_items:
self.group_selected_items[group_name] = []
self.group_areas[group_name] = QScrollArea(self)
self.group_main_widget[group_name] = QWidget(
self.group_areas[group_name])
self.group_label[group_name] = QLabel(group_name, self)
self.group_label[group_name].setAlignment(Qt.AlignCenter)
self.main_vlayout.addWidget(self.group_label[group_name])
self.main_vlayout.addWidget(self.group_areas[group_name])
self.group_selection_vlayout[group_name] = QVBoxLayout(self)
self.group_item_all[group_name] = MyQCheckBox(
"All", self, self.group_selection_vlayout[group_name], None)
self.MakeCheckBoxList(self.group_selection_vlayout[group_name],
self.group_selected_items[group_name],
self.history_items[group_name],
self.group_item_all[group_name])
self.group_main_widget[group_name].setLayout(
self.group_selection_vlayout[group_name])
self.group_areas[group_name].setWidget(
self.group_main_widget[group_name])
self.clear_button = QPushButton("Clear Selection", self)
self.clear_button.clicked.connect(self.onClearClicked)
self.choose_button = QPushButton("Get Last Export Choose", self)
self.choose_button.clicked.connect(self.onButtonChooseCliked)
# self.main_vlayout.addWidget(self.choose_button)
self.label4 = QLabel("Window time", self)
self.label4.setAlignment(Qt.AlignCenter)
# self.main_vlayout.addWidget(self.label4)
self.window = QLineEdit(self)
# self.main_vlayout.addWidget(self.window)
self.window.setText("3")
self.label5 = QLabel("Duration Time:", self)
self.label5.setAlignment(Qt.AlignCenter)
self.label6 = QLabel("Step:", self)
self.label6.setAlignment(Qt.AlignCenter)
self.step = QLineEdit(self)
self.step.setText("1")
self.windows_time_3 = QHBoxLayout(self)
self.windows_time_3.addWidget(self.label4)
self.windows_time_3.addWidget(self.window)
self.windows_time_3.addWidget(self.label5)
self.main_vlayout.addLayout(self.windows_time_3)
self.step_2 = QHBoxLayout(self)
self.step_2.addWidget(self.label6)
self.step_2.addWidget(self.step)
self.main_vlayout.addLayout(self.step_2)
# self.main_vlayout.addRow(self.label4, self.window)
self.two_buttons = QHBoxLayout(self)
self.two_buttons.addWidget(self.choose_button)
self.two_buttons.addWidget(self.clear_button)
self.main_vlayout.addLayout(self.two_buttons)
# self.main_vlayout.addRow(self.clear_button, self.choose_button)
# self.label5 = QLabel("Load CSV", self)
# self.label5.setAlignment(Qt.AlignCenter)
#
# self.main_vlayout.addWidget(self.label5)
# self.label6 = QLabel("output", self)
# self.label6.setAlignment(Qt.AlignCenter)
#
# self.main_vlayout.addWidget(self.label6)
self.save_button = QPushButton("Save CSV...", self)
self.save_button.clicked.connect(self.onSaveClicked)
self.submit_button = QPushButton("Sumbit", self)
self.submit_button.clicked.connect(self.onSumbitClicked)
# self.main_vlayout.addWidget(self.save_button)
self.two_buttons2 = QHBoxLayout(self)
self.two_buttons2.addWidget(self.save_button)
self.two_buttons2.addWidget(self.save_path)
self.main_vlayout.addLayout(self.two_buttons2)
# self.main_vlayout.addRow(self.save_button, self.client_answers1)
self.main_vlayout.addWidget(self.submit_button)
self.show()
def get_min_rows_csv(self, csv_files):
import pandas as pd
dfs = map(lambda x: pd.read_csv(x, header=0), csv_files)
number = dfs[0].shape[0]
number = reduce(lambda acc, curr: min(acc, curr.shape[0]), dfs, number)
self.label5.setText("Duration Time: %s" % number)
def onClearClicked(self):
self.clearTopicCheckState()
def clearTopicCheckState(self):
for item in self.items_list:
item.setCheckState(False)
for item in self.group_item_all.values():
item.setCheckState(False)
def MakeCheckBoxList(self, selection_vlayout, selected, topics_Keys,
item_all):
item_all.stateChanged.connect(
lambda x: self.updateList(x, item_all, None))
selection_vlayout.addWidget(item_all)
topic_data_list = topics_Keys
topic_data_list.sort()
for topic in topic_data_list:
self.addCheckBox(topic, selection_vlayout, selected)
def addCheckBox(self, topic, selection_vlayout, selected_list):
item = MyQCheckBox(topic, self, selection_vlayout, selected_list)
item.stateChanged.connect(lambda x: self.updateList(x, item, topic))
self.items_list.append(item)
selection_vlayout.addWidget(item)
def updateList(self,
state,
item_clicked,
topic=None,
force_update_state=False):
if type(item_clicked) is str:
item_clicked = self.get_item_by_name(item_clicked)
if item_clicked is None:
return
if topic is None: # The "All" checkbox was checked / unchecked
# print "if topic is None"
if state == Qt.Checked:
# self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Unchecked and \
item.selection_vlayout == item_clicked.selection_vlayout:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
# self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Checked and \
item.selection_vlayout == item_clicked.selection_vlayout:
item.setCheckState(Qt.Unchecked)
else:
if state == Qt.Checked:
item_clicked.selected_list.append(topic)
# print item_clicked.selected_list
else:
item_clicked.selected_list.remove(topic)
# if self.item_all.checkState() == Qt.Checked:
# self.item_all.setCheckState(Qt.PartiallyChecked)
def get_current_opened_directory(self, filepath):
import os
direc = "/"
if os.path.isfile(filepath):
with open(filepath, 'r') as f:
direc = f.read()
return direc
def onButtonClicked(self):
import inspect, os
filepath = os.path.dirname(
os.path.abspath(inspect.getfile(
inspect.currentframe()))) + "/log/select_history.log"
fd = QFileDialog(self)
wc = "Csv files {.csv} (*.csv)"
self.files = []
current_directory = self.get_current_opened_directory(filepath)
tmp_pathes, filter = fd.getOpenFileNamesAndFilter(
filter=wc, initialFilter=('*.csv'), directory=current_directory)
for path in tmp_pathes:
self.files.append(path.encode("utf-8"))
# print self.files
if len(self.files) != 0:
self.get_min_rows_csv(tmp_pathes)
self.select_path.setText(self.files[0])
with open(filepath, "w") as f:
f.write(self.files[0])
# handler = logging.FileHandler(filepath, mode='w')
# logger_topic.addHandler(handler)
# logger_topic.info(self.files[0])
else:
self.select_path.setText("")
def check_int(self, number, condition, title, message_body):
try:
# print type(number)
val = int(number)
if val <= condition:
QMessageBox.about(self, title,
"The number should > %s" % condition)
return False
except ValueError:
QMessageBox.about(self, title, message_body)
return False
return True
def check_choose(self):
flag = True
temp = []
for item in self.group_selected_items.values():
if item:
for i in item:
temp.append(i)
if len(temp) == 0:
QMessageBox.about(self, "Features",
"One feature at least should be chosen")
flag = False
return flag
def check_files_amount(self):
flag = True
if len(self.files) <= 0:
QMessageBox.about(self, "Load CSV",
"One file at least should be chosen")
flag = False
return flag
def check_validation(self):
flag = self.check_files_amount()
flag = flag & self.check_choose()
flag = flag & self.check_int(self.window.text(), 2,
"Error in Window Time",
"That's not a number!")
flag = flag & self.check_int(self.step.text(), 0, "Error in Step",
"That's not a integer!")
# TODO selected topic not empty
if self.saved_dir == []:
QMessageBox.about(self, "Save CSV", "Select path for saving")
flag = False
return flag
def onSumbitClicked(self):
if not self.check_validation():
return
self.selected_topics = []
for item in self.group_selected_items.values():
if item:
for i in item:
self.selected_topics.append(i)
# Defined Logging
# handler = logging.FileHandler('/var/tmp/logger_history.log', mode='a')
# logger_topic.addHandler(handler)
topics = self.selected_topics
with open(get_path() + 'logger_history.log', "w") as f:
for topic in topics:
f.write(topic + "\n")
self.createTimeSeriesFeatures(self.files, self.saved_dir,
int(self.window.text()),
self.group_selected_items,
int(self.step.text()))
def onSaveClicked(self):
import inspect, os
filepath = os.path.dirname(
os.path.abspath(inspect.getfile(
inspect.currentframe()))) + "/log/save_history.log"
# print self.files
# print self.window.text()
# print self.group_selected_items.values()
# print self.group_selected_items
current_directory = self.get_current_opened_directory(filepath)
# print current_directory
# self._to_save_filename = QFileDialog.getSaveFileName(self, self.tr('csv File'), current_directory,
# self.tr('csv (*.csv)'))
self.saved_dir = str(
QFileDialog.getExistingDirectory(self, "Select Directory",
current_directory))
# if self._to_save_filename[0] != "":
# with open(filepath, "w") as f:
# f.write(self._to_save_filename[0])
if self.saved_dir != "":
with open(filepath, "w") as f:
f.write(self.saved_dir)
# handler = logging.FileHandler(filepath, mode='w')
# logger_topic.addHandler(handler)
# print self._to_save_filename[0]
# logger_topic.info()
# self.save_path.setText(get_corrent_file_name(self._to_save_filename[0], ".csv"))
self.save_path.setText(self.saved_dir)
def createTimeSeriesFeatures(self, files, saved_dir, window,
group_selected_items, step):
import TimeSeriesFeatures as TS
# saved_dir = saved_dir[0].encode('utf-8')
for input_path in files:
output_path = generate_csv_from_bag(saved_dir, input_path)
print "++++++++++++++++++++++++++ ", output_path
print "in = %s out = %s " % (input_path, output_path)
print "step = %s" % step
ts = TS.TimeSeries(input_path, output_path, window,
group_selected_items, step)
ts.generate_time_series_features()
QMessageBox.about(self, "csv save", "csv was saved successfuly")
def onButtonChooseCliked(self):
for checkbox in self.items_list:
checkbox.setCheckState(Qt.Unchecked)
with open(get_path() + "logger_history.log", 'r') as f:
topics = f.read().splitlines()
for checkbox in self.items_list:
if checkbox.text() in topics:
checkbox.setCheckState(Qt.Checked)
class UAVStatePublisher(QMainWindow):
def __init__(self):
super(UAVStatePublisher, self).__init__()
#Not working: how to set window dimension
self.height = 300
self.width = 300
self.top = 50
self.left = 50
font = QFont("Helvetica", 9, QFont.Bold)
rospy.init_node("uav_state_publisher")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.arm_disarm_req = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.takeoff_req = rospy.ServiceProxy('/mavros/cmd/takeoff',
CommandTOL)
self.land_req = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL)
self.offboard_req = rospy.ServiceProxy('/mavros/set_mode', SetMode)
rospy.Subscriber("/mavros/local_position/odom", Odometry, self.odomCb)
rospy.Subscriber("/mavros/state", State, self.stateCb)
self.state = "Idle"
self.arm_state = False
self.local_cmd = ([0.0, 0.0, 0.0])
self.dx_cmd = 0.0
self.dy_cmd = 0.0
self.dz_cmd = 0.0
self.yaw_cmd = 0.0
self.ctrl_mode = "position"
self.setpoint_raw_pub = rospy.Publisher('mavros/setpoint_raw/local',
PositionTarget,
queue_size=1)
'''
uint16 IGNORE_PX=1
uint16 IGNORE_PY=2
uint16 IGNORE_PZ=4
uint16 IGNORE_VX=8
uint16 IGNORE_VY=16
uint16 IGNORE_VZ=32
uint16 IGNORE_AFX=64
uint16 IGNORE_AFY=128
uint16 IGNORE_AFZ=256
uint16 FORCE=512
uint16 IGNORE_YAW=1024
uint16 IGNORE_YAW_RATE=2048
'''
self.pos_raw_msg = PositionTarget()
self.pos_raw_msg.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
self.pos_raw_msg.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW + PositionTarget.FORCE
self.vel_raw_msg = PositionTarget()
self.vel_raw_msg.coordinate_frame = PositionTarget.FRAME_BODY_NED
self.vel_raw_msg.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW + PositionTarget.FORCE
window = QWidget()
self.layout = QVBoxLayout()
self.setWindowTitle("UAV State Publisher")
#Position label
self.textbox_state = QLabel(self)
self.textbox_state.move(20, 20)
self.textbox_state.resize(280, 40)
self.textbox_state.setFont(font)
#Position label
self.textbox_pos = QLabel(self)
self.textbox_pos.move(20, 20)
self.textbox_pos.resize(280, 40)
#Yaw Label
self.textbox_yaw = QLabel(self)
self.textbox_yaw.move(20, 20)
self.textbox_yaw.resize(280, 40)
#Command position label
self.textbox_cmd_pos = QLabel(self)
self.textbox_cmd_pos.move(20, 20)
self.textbox_cmd_pos.resize(280, 40)
#Command yaw label
self.textbox_cmd_yaw = QLabel(self)
self.textbox_cmd_yaw.move(20, 20)
self.textbox_cmd_yaw.resize(280, 40)
#Position control
self.checkbox_pvcontrol = QCheckBox('Position/velocity control')
#self.checkbox_vcontrol = QCheckBox('Velocity control')
self.checkbox_pvcontrol.stateChanged.connect(
self.select_chkbox_pvcontrol)
#self.checkbox_vcontrol.stateChanged.connect(self.select_chkbox_vcontrol)
self.arm_button = QPushButton('Arm / Disarm', self)
self.arm_button.setToolTip('Start/Stop motor spinning')
self.arm_button.move(100, 70)
self.arm_button.clicked.connect(self.on_click_arm)
self.toff_button = QPushButton('Take-off', self)
self.toff_button.setToolTip('takeoff the UAV')
self.toff_button.move(100, 70)
self.toff_button.clicked.connect(self.on_click_toff)
self.land_button = QPushButton('Land', self)
self.land_button.setToolTip('Land the UAV')
self.land_button.move(100, 70)
self.land_button.clicked.connect(self.on_click_land)
self.offboard_button = QPushButton('OFFBOARD', self)
self.offboard_button.setToolTip('Start offboard mode')
self.offboard_button.move(100, 70)
self.offboard_button.clicked.connect(self.on_click_offboard)
x_cmdlabel = QLabel("Slide to command the x rate (d/s)")
x_cmdlabel.setFont(font)
self.cmd_dx_slider = QSlider(Qt.Horizontal, self)
self.cmd_dx_slider.setGeometry(30, 40, 200, 30)
self.cmd_dx_slider.valueChanged[int].connect(self.changeValue_dx)
self.cmd_dx_slider.setRange(-10, 10)
self.cmd_dx_slider.setValue(0)
y_cmdlabel = QLabel("Slide to command the y rate (d/s)")
y_cmdlabel.setFont(font)
self.cmd_dy_slider = QSlider(Qt.Horizontal, self)
self.cmd_dy_slider.setGeometry(30, 40, 200, 30)
self.cmd_dy_slider.valueChanged[int].connect(self.changeValue_dy)
self.cmd_dy_slider.setRange(-10, 10)
self.cmd_dy_slider.setValue(0)
z_cmdlabel = QLabel("Slide to command the z rate (d/s)")
z_cmdlabel.setFont(font)
self.cmd_dz_slider = QSlider(Qt.Horizontal, self)
self.cmd_dz_slider.setGeometry(30, 40, 200, 30)
self.cmd_dz_slider.valueChanged[int].connect(self.changeValue_dz)
self.cmd_dz_slider.setRange(-10, 10)
self.cmd_dz_slider.setValue(0)
self.center_slider_button = QPushButton('Center', self)
self.center_slider_button.setToolTip('Put in the middel all sliders')
self.center_slider_button.move(100, 70)
self.center_slider_button.clicked.connect(self.on_click_center)
yaw_cmdlabel = QLabel("Slide to command the yaw rate (d/s)")
yaw_cmdlabel.setFont(font)
self.cmd_dyaw_slider = QSlider(Qt.Horizontal, self)
self.cmd_dyaw_slider.setGeometry(30, 40, 200, 30)
self.cmd_dyaw_slider.valueChanged[int].connect(self.changeValue_dyaw)
self.cmd_dyaw_slider.setRange(-60, 60)
self.cmd_dyaw_slider.setValue(0)
joy_cmdlabel = QLabel("Click and release buttons to command the UAV")
joy_cmdlabel.setFont(font)
self.forward_button = QPushButton('FORWARD', self)
self.forward_button.setToolTip('Move robot in positive x direction')
self.forward_button.move(100, 70)
self.forward_button.pressed.connect(self.on_pres_forward)
self.forward_button.released.connect(self.on_rel_forward)
self.back_button = QPushButton('BACKWARD', self)
self.back_button.setToolTip('Move robot in negative x direction')
self.back_button.move(100, 70)
self.back_button.pressed.connect(self.on_pres_back)
self.back_button.released.connect(self.on_rel_back)
self.left_button = QPushButton('LEFT', self)
self.left_button.setToolTip('Move robot in positive y direction')
self.left_button.move(100, 70)
self.left_button.pressed.connect(self.on_pres_left)
self.left_button.released.connect(self.on_rel_left)
self.right_button = QPushButton('RIGHT', self)
self.right_button.setToolTip('Move robot in negative y direction')
self.right_button.move(100, 70)
self.right_button.pressed.connect(self.on_pres_right)
self.right_button.released.connect(self.on_rel_right)
self.up_button = QPushButton('UP', self)
self.up_button.setToolTip('Move robot in positive z direction')
self.up_button.move(100, 70)
self.up_button.pressed.connect(self.on_pres_up)
self.up_button.released.connect(self.on_rel_up)
self.down_button = QPushButton('DOWN', self)
self.down_button.setToolTip('Move robot in negative z direction')
self.down_button.move(100, 70)
self.down_button.pressed.connect(self.on_pres_down)
self.down_button.released.connect(self.on_rel_down)
self.rot_left_button = QPushButton('ROTATE_LEFT', self)
self.rot_left_button.setToolTip('rotate robot in left direction')
self.rot_left_button.move(100, 70)
self.rot_left_button.pressed.connect(self.on_pres_yawleft)
self.rot_left_button.released.connect(self.on_rel_yawleft)
self.rot_right_button = QPushButton('ROTATE_RIGHT', self)
self.rot_right_button.setToolTip('rotate robot in right direction')
self.rot_right_button.move(100, 70)
self.rot_right_button.pressed.connect(self.on_pres_yawright)
self.rot_right_button.released.connect(self.on_rel_yawright)
self.layout.addWidget(self.textbox_state)
self.layout.addWidget(self.textbox_pos)
self.layout.addWidget(self.textbox_yaw)
self.layout.addWidget(self.textbox_cmd_pos)
self.layout.addWidget(self.textbox_cmd_yaw)
self.layout.addWidget(self.arm_button)
self.layout.addWidget(self.toff_button)
self.layout.addWidget(self.land_button)
self.layout.addWidget(self.offboard_button)
self.layout.addWidget(self.checkbox_pvcontrol)
#self.layout.addWidget(self.checkbox_vcontrol)
self.layout.addWidget(x_cmdlabel)
self.layout.addWidget(self.cmd_dx_slider)
self.layout.addWidget(y_cmdlabel)
self.layout.addWidget(self.cmd_dy_slider)
self.layout.addWidget(z_cmdlabel)
self.layout.addWidget(self.cmd_dz_slider)
self.layout.addWidget(yaw_cmdlabel)
self.layout.addWidget(self.cmd_dyaw_slider)
self.layout.addWidget(self.center_slider_button)
self.layout.addWidget(joy_cmdlabel)
self.layout.addWidget(self.forward_button)
self.layout.addWidget(self.back_button)
self.layout.addWidget(self.left_button)
self.layout.addWidget(self.right_button)
self.layout.addWidget(self.up_button)
self.layout.addWidget(self.down_button)
self.layout.addWidget(self.rot_left_button)
self.layout.addWidget(self.rot_right_button)
self.checkbox_pvcontrol.setChecked(True)
self.forward_button.setEnabled(False)
self.back_button.setEnabled(False)
self.left_button.setEnabled(False)
self.right_button.setEnabled(False)
self.up_button.setEnabled(False)
self.down_button.setEnabled(False)
self.rot_left_button.setEnabled(False)
self.rot_right_button.setEnabled(False)
window.setLayout(self.layout)
window.show()
app.exec_()
sys.exit()
'''
self.setWindowTitle("UAV State Publisher")
font = QFont("Helvetica", 9, QFont.Bold)
label = QLabel("Yaw")
label.setFont(font)
addWidget(label)
mySlider = QSlider(Qt.Horizontal, self)
mySlider.setGeometry(30, 40, 200, 30)
mySlider.valueChanged[int].connect(self.changeValue)
#self.setGeometry(50,50,320,200)
self.show()
'''
def select_chkbox_pvcontrol(self, state):
if state == 0:
self.cmd_dx_slider.setValue(0)
self.cmd_dy_slider.setValue(0)
self.cmd_dz_slider.setValue(0)
self.cmd_dyaw_slider.setValue(0)
self.center_slider_button.setEnabled(False)
self.cmd_dx_slider.setEnabled(False)
self.cmd_dy_slider.setEnabled(False)
self.cmd_dz_slider.setEnabled(False)
self.cmd_dyaw_slider.setEnabled(False)
self.forward_button.setEnabled(True)
self.back_button.setEnabled(True)
self.left_button.setEnabled(True)
self.right_button.setEnabled(True)
self.up_button.setEnabled(True)
self.down_button.setEnabled(True)
self.rot_left_button.setEnabled(True)
self.rot_right_button.setEnabled(True)
self.ctrl_mode = "velocity"
elif state == 2:
self.cmd_dx_slider.setValue(0)
self.cmd_dy_slider.setValue(0)
self.cmd_dz_slider.setValue(0)
self.cmd_dyaw_slider.setValue(0)
self.center_slider_button.setEnabled(True)
self.cmd_dx_slider.setEnabled(True)
self.cmd_dy_slider.setEnabled(True)
self.cmd_dz_slider.setEnabled(True)
self.cmd_dyaw_slider.setEnabled(True)
self.forward_button.setEnabled(False)
self.back_button.setEnabled(False)
self.left_button.setEnabled(False)
self.right_button.setEnabled(False)
self.up_button.setEnabled(False)
self.down_button.setEnabled(False)
self.rot_left_button.setEnabled(False)
self.rot_right_button.setEnabled(False)
self.ctrl_mode = "position"
#def select_chkbox_pcontrol(self, state):
# print("select_chkbox_pcontrol state: ", state)
# self.checkbox_vcontrol.setChecked( False )
# self.checkbox_pcontrol.setChecked( True )
#def select_chkbox_vcontrol( self, state ):
# print("select_chkbox_vcontrol state: ", state)
# self.checkbox_pcontrol.setChecked( False )
# self.checkbox_vcontrol.setChecked( True )
def on_pres_yawleft(self):
self.yaw_cmd = 0.3
def on_rel_yawleft(self):
self.yaw_cmd = 0.0
def on_pres_yawright(self):
self.yaw_cmd = -0.3
def on_rel_yawright(self):
self.yaw_cmd = 0.0
def on_pres_forward(self):
self.dy_cmd = 0.3
def on_rel_forward(self):
self.dy_cmd = 0.0
def on_pres_back(self):
self.dy_cmd = -0.3
def on_rel_back(self):
self.dy_cmd = 0.0
def on_pres_left(self):
self.dx_cmd = -0.3
def on_rel_left(self):
self.dx_cmd = 0.0
def on_pres_right(self):
self.dx_cmd = 0.3
def on_rel_right(self):
self.dx_cmd = 0.0
def on_pres_up(self):
self.dz_cmd = 0.3
def on_rel_up(self):
self.dz_cmd = 0.0
def on_pres_down(self):
self.dz_cmd = -0.3
def on_rel_down(self):
self.dz_cmd = 0.0
def on_click_arm(self):
if self.arm_state == True:
self.arm_disarm_req(False)
else:
self.arm_disarm_req(True)
def on_click_toff(self):
print("Request Takeoff") #Todo
self.local_cmd[2] = self.local_cmd[2] + 2.0
def on_click_land(self):
print("Request Land") #Todo
self.land_req(altitude=0, latitude=0, longitude=0, min_pitch=0, yaw=0)
def on_click_center(self):
self.cmd_dx_slider.setValue(0)
self.cmd_dy_slider.setValue(0)
self.cmd_dz_slider.setValue(0)
self.cmd_dyaw_slider.setValue(0)
def on_click_offboard(self):
self.offboard_req(base_mode=0, custom_mode="OFFBOARD")
def changeValue_dyaw(self, value):
self.yaw_cmd = (value * 3.1415) / 180.0
def changeValue_dx(self, value):
self.dx_cmd = value / 10.0
def changeValue_dy(self, value):
self.dy_cmd = value / 10.0
def changeValue_dz(self, value):
self.dz_cmd = value / 10.0
def stateCb(self, msg):
self.state = msg.mode
self.arm_state = msg.armed
def odomCb(self, msg):
self.textbox_state.setText("State: " + self.state)
str_x = str(round(msg.pose.pose.position.x, 2))
str_y = str(round(msg.pose.pose.position.y, 2))
str_z = str(round(msg.pose.pose.position.z, 2))
coords = str_x + " " + str_y + " " + str_z
self.textbox_pos.setText("Pos: " + coords)
quaternion = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
yaw = euler[2]
self.textbox_yaw.setText("Yaw: " + str(round(yaw, 2)))
self.pos_raw_msg.header.stamp = rospy.get_rostime()
if (self.state == "OFFBOARD"):
if self.ctrl_mode == "position":
self.local_cmd[0] = self.local_cmd[0] + self.dx_cmd * 0.02
self.local_cmd[1] = self.local_cmd[1] + self.dy_cmd * 0.02
self.local_cmd[2] = self.local_cmd[2] + self.dz_cmd * 0.02
else:
self.local_cmd[0] = msg.pose.pose.position.x
self.local_cmd[1] = msg.pose.pose.position.y
self.local_cmd[2] = msg.pose.pose.position.z
#self.yaw_cmd = 0.0
else:
self.yaw_cmd = yaw
self.local_cmd[0] = msg.pose.pose.position.x
self.local_cmd[1] = msg.pose.pose.position.y
self.local_cmd[2] = msg.pose.pose.position.z
if self.ctrl_mode == "position":
self.pos_raw_msg.position.x = self.local_cmd[0]
self.pos_raw_msg.position.y = self.local_cmd[1]
self.pos_raw_msg.position.z = self.local_cmd[2]
self.pos_raw_msg.yaw_rate = self.yaw_cmd
self.textbox_cmd_pos.setText("Cmd Pos: " +
str(round(self.local_cmd[0], 2)) +
" " +
str(round(self.local_cmd[1], 2)) +
" " +
str(round(self.local_cmd[2], 2)))
self.textbox_cmd_yaw.setText("Cmd dYaw: " +
str(round(self.yaw_cmd, 2)) +
" rad/s")
self.setpoint_raw_pub.publish(self.pos_raw_msg)
elif self.ctrl_mode == "velocity":
self.vel_raw_msg.velocity.x = self.dx_cmd
self.vel_raw_msg.velocity.y = self.dy_cmd
self.vel_raw_msg.velocity.z = self.dz_cmd
self.vel_raw_msg.yaw_rate = self.yaw_cmd
self.textbox_cmd_pos.setText("Cmd Vel: " + str(self.dx_cmd) + " " +
str(self.dy_cmd) + " " +
str(self.dz_cmd))
self.textbox_cmd_yaw.setText("Cmd dYaw: " +
str(round(self.yaw_cmd, 2)) +
" rad/s")
self.setpoint_raw_pub.publish(self.vel_raw_msg)
def init_sliders(self):
sliderbox = self._widget.findChild(QLayout, 'Sliders')
graph_button = QPushButton()
graph_button.setCheckable(True)
graph_button.setText("Graph Off")
graph_button.toggle()
graph_button.clicked.connect(self.set_graph_state)
self.graph_button = graph_button
firstCol = QVBoxLayout()
firstCol.addWidget(graph_button)
sliderbox.addLayout(firstCol)
self.sliders = []
all_rows_layout = QVBoxLayout()
chan_idx = 0
for num_channels_row in self.settings['num_channels']:
row_layout = QHBoxLayout()
for i in range(num_channels_row):
idx = chan_idx * 1
slider_group = {
'slider_p': None,
'number_p': None,
'slider_v': None,
'number_v': None,
'on_off': None
}
layout_cluster = QVBoxLayout()
slider_cluster = QHBoxLayout()
label = QLabel()
label.setText("Chan. %d" % (idx + 1))
label.setAlignment(Qt.AlignCenter)
layout_cluster.addWidget(label)
for j in range(2):
layout = QVBoxLayout()
layout.setAlignment(Qt.AlignHCenter)
slider = QSlider(Qt.Vertical)
slider.setMinimum(0)
slider.setMaximum(255)
slider.setValue(
self.settings['valve_offsets'][chan_idx][j])
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(5)
spinbox = QSpinBox()
spinbox.setRange(0, 255)
spinbox.setValue(
self.settings['valve_offsets'][chan_idx][j])
slider.valueChanged.connect(spinbox.setValue)
spinbox.valueChanged.connect(slider.setValue)
cb_function_curr = lambda value, idx=idx: self.send_slider_value(
idx, value)
slider.valueChanged.connect(cb_function_curr)
label = QLabel()
label.setAlignment(Qt.AlignCenter)
if j == 0:
slider_group['slider_p'] = slider
slider_group['number_p'] = spinbox
label.setText("P")
else:
slider_group['slider_v'] = slider
slider_group['number_v'] = spinbox
label.setText("V")
labelmax = QLabel()
labelmax.setAlignment(Qt.AlignCenter)
labelmax.setText("255")
labelmin = QLabel()
labelmin.setAlignment(Qt.AlignCenter)
labelmin.setText("0")
layout.addWidget(label)
layout.addWidget(labelmax)
layout.addWidget(slider, Qt.AlignHCenter)
layout.addWidget(labelmin)
layout.addWidget(spinbox, Qt.AlignHCenter)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
slider_cluster.addLayout(layout)
on_button = QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][chan_idx]:
on_button.toggle()
on_button.setText("On")
on_button.clicked.connect(
lambda state, idx=idx: self.send_channel_state(idx, state))
slider_group['on_off'] = on_button
layout_cluster.addLayout(slider_cluster)
layout_cluster.addWidget(on_button)
row_layout.addLayout(layout_cluster)
row_layout.addSpacing(20)
self.sliders.append(slider_group)
chan_idx += 1
all_rows_layout.addLayout(row_layout)
sliderbox.addLayout(all_rows_layout)
def init_sliders(self):
sliderbox = self._widget.findChild(QLayout, 'Sliders')
firstCol = QVBoxLayout()
graph_button = QPushButton()
graph_button.setCheckable(True)
graph_button.setText("Graph Off")
graph_button.toggle()
graph_button.clicked.connect(self.set_graph_state)
reset_button = QPushButton()
reset_button.setCheckable(False)
reset_button.setText("Reset")
reset_button.clicked.connect(self.set_reset)
self.graph_button = graph_button
self.reset_button = reset_button
firstCol.addWidget(graph_button)
firstCol.addWidget(reset_button)
firstCol.setAlignment(graph_button, Qt.AlignVCenter)
firstCol.setAlignment(reset_button, Qt.AlignVCenter)
zero_button = QPushButton()
zero_button.setCheckable(False)
zero_button.setText("Set All Zero")
zero_button.clicked.connect(self.set_pressure_zero)
self.zero_button = zero_button
firstCol.addWidget(zero_button)
firstCol.setAlignment(zero_button, Qt.AlignVCenter)
transition_box = QVBoxLayout()
label = QLabel()
label.setAlignment(Qt.AlignCenter)
label.setText("Transition Time")
spinbox = QDoubleSpinBox()
spinbox.setMinimum(0)
spinbox.setMaximum(10)
spinbox.setValue(self.settings['transitions'])
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
spinbox.setSuffix(" sec")
spinbox.valueChanged.connect(self.set_transition_value)
transition_box.addWidget(label)
transition_box.addWidget(spinbox)
transition_box.setAlignment(label, Qt.AlignBottom)
transition_box.setAlignment(spinbox, Qt.AlignTop)
firstCol.addLayout(transition_box)
self.sliders = []
sliderbox.addLayout(firstCol)
all_rows_layout = QVBoxLayout()
chan_idx = 0
for num_channels_row in self.settings['num_channels']:
row_layout = QHBoxLayout()
for i in range(num_channels_row):
idx = chan_idx * 1
slider_group = {'slider': None, 'number': None, 'on_off': None}
layout_cluster = QVBoxLayout()
layout = QVBoxLayout()
layout.setAlignment(Qt.AlignHCenter)
slider = QSlider(Qt.Vertical)
slider.setMinimum(self.settings['min_pressure'][idx] * 10.0)
slider.setMaximum(self.settings['max_pressure'][idx] * 10.0)
slider.setValue(0)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(20)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(self.settings['min_pressure'][idx])
spinbox.setMaximum(self.settings['max_pressure'][idx])
spinbox.setValue(0)
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
cb_function_curr = lambda value, idx=idx, slider=False: self.send_slider_value(
idx, value, slider)
cb_function_curr2 = lambda value, idx=idx, slider=True: self.send_slider_value(
idx, value, slider)
slider.valueChanged.connect(cb_function_curr2)
spinbox.valueChanged.connect(cb_function_curr)
labelmax = QLabel()
labelmax.setAlignment(Qt.AlignCenter)
labelmax.setText("%0.1f" %
(self.settings['max_pressure'][idx]))
labelmin = QLabel()
labelmin.setAlignment(Qt.AlignCenter)
labelmin.setText("%0.1f" %
(self.settings['min_pressure'][idx]))
layout.addWidget(labelmax)
layout.addWidget(slider)
layout.addWidget(labelmin)
layout.addWidget(spinbox)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
label = QLabel()
label.setText("Chan. %d" % (chan_idx + 1))
label.setAlignment(Qt.AlignCenter)
layout_cluster.addWidget(label)
layout_cluster.addLayout(layout)
slider_group['slider'] = slider
slider_group['number'] = spinbox
on_button = QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][idx]:
on_button.toggle()
on_button.setText("On")
on_button.clicked.connect(
lambda state, idx=idx: self.send_channel_state(idx, state))
slider_group['on_off'] = on_button
layout_cluster.addWidget(on_button)
row_layout.addLayout(layout_cluster)
row_layout.addSpacing(20)
self.sliders.append(slider_group)
chan_idx += 1
all_rows_layout.addLayout(row_layout)
sliderbox.addLayout(all_rows_layout)
class GroupWidget(QWidget):
'''
(Isaac's guess as of 12/13/2012)
This class bonds multiple Editor instances that are associated with
a single node as a group.
'''
# public signal
sig_node_disabled_selected = Signal(str)
sig_node_state_change = Signal(bool)
def __init__(self, updater, config, nodename):
'''
:param config:
:type config: Dictionary? defined in dynamic_reconfigure.client.Client
:type nodename: str
'''
super(GroupWidget, self).__init__()
self.state = config['state']
self.param_name = config['name']
self._toplevel_treenode_name = nodename
# TODO: .ui file needs to be back into usage in later phase.
# ui_file = os.path.join(rp.get_path('rqt_reconfigure'),
# 'resource', 'singlenode_parameditor.ui')
# loadUi(ui_file, self)
verticalLayout = QVBoxLayout(self)
verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))
_widget_nodeheader = QWidget()
_h_layout_nodeheader = QHBoxLayout(_widget_nodeheader)
_h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))
self.nodename_qlabel = QLabel(self)
font = QFont('Trebuchet MS, Bold')
font.setUnderline(True)
font.setBold(True)
# Button to close a node.
_icon_disable_node = QIcon.fromTheme('window-close')
_bt_disable_node = QPushButton(_icon_disable_node, '', self)
_bt_disable_node.setToolTip('Hide this node')
_bt_disable_node_size = QSize(36, 24)
_bt_disable_node.setFixedSize(_bt_disable_node_size)
_bt_disable_node.pressed.connect(self._node_disable_bt_clicked)
_h_layout_nodeheader.addWidget(self.nodename_qlabel)
_h_layout_nodeheader.addWidget(_bt_disable_node)
self.nodename_qlabel.setAlignment(Qt.AlignCenter)
font.setPointSize(10)
self.nodename_qlabel.setFont(font)
grid_widget = QWidget(self)
self.grid = QFormLayout(grid_widget)
verticalLayout.addWidget(_widget_nodeheader)
verticalLayout.addWidget(grid_widget, 1)
# Again, these UI operation above needs to happen in .ui file.
self.tab_bar = None # Every group can have one tab bar
self.tab_bar_shown = False
self.updater = updater
self.editor_widgets = []
self._param_names = []
self._create_node_widgets(config)
rospy.logdebug('Groups node name={}'.format(nodename))
self.nodename_qlabel.setText(nodename)
# Labels should not stretch
#self.grid.setColumnStretch(1, 1)
#self.setLayout(self.grid)
def collect_paramnames(self, config):
pass
def _create_node_widgets(self, config):
'''
:type config: Dict?
'''
i_debug = 0
for param in config['parameters']:
begin = time.time() * 1000
editor_type = '(none)'
if param['edit_method']:
widget = EnumEditor(self.updater, param)
elif param['type'] in EDITOR_TYPES:
rospy.logdebug('GroupWidget i_debug=%d param type =%s',
i_debug,
param['type'])
editor_type = EDITOR_TYPES[param['type']]
widget = eval(editor_type)(self.updater, param)
self.editor_widgets.append(widget)
self._param_names.append(param['name'])
rospy.logdebug('groups._create_node_widgets num editors=%d',
i_debug)
end = time.time() * 1000
time_elap = end - begin
rospy.logdebug('ParamG editor={} loop=#{} Time={}msec'.format(
editor_type, i_debug, time_elap))
i_debug += 1
for name, group in config['groups'].items():
if group['type'] == 'tab':
widget = TabGroup(self, self.updater, group, self._toplevel_treenode_name)
elif group['type'] in _GROUP_TYPES.keys():
widget = eval(_GROUP_TYPES[group['type']])(self.updater, group, self._toplevel_treenode_name)
else:
widget = eval(_GROUP_TYPES[''])(self.updater, group, self._toplevel_treenode_name)
self.editor_widgets.append(widget)
rospy.logdebug('groups._create_node_widgets ' +
'name=%s',
name)
for i, ed in enumerate(self.editor_widgets):
ed.display(self.grid)
rospy.logdebug('GroupWdgt._create_node_widgets len(editor_widgets)=%d',
len(self.editor_widgets))
def display(self, grid):
grid.addRow(self)
def update_group(self, config):
if 'state' in config:
old_state = self.state
self.state = config['state']
if self.state != old_state:
self.sig_node_state_change.emit(self.state)
names = [name for name in config.keys()]
for widget in self.editor_widgets:
if isinstance(widget, EditorWidget):
if widget.param_name in names:
widget.update_value(config[widget.param_name])
elif isinstance(widget, GroupWidget):
cfg = find_cfg(config, widget.param_name)
widget.update_group(cfg)
def close(self):
for w in self.editor_widgets:
w.close()
def get_treenode_names(self):
'''
:rtype: str[]
'''
return self._param_names
def _node_disable_bt_clicked(self):
rospy.logdebug('param_gs _node_disable_bt_clicked')
self.sig_node_disabled_selected.emit(self._toplevel_treenode_name)
class GroupWidget(QWidget):
'''
(Isaac's guess as of 12/13/2012)
This class bonds multiple Editor instances that are associated with
a single node as a group.
'''
# public signal
sig_node_disabled_selected = Signal(str)
def __init__(self, updater, config, nodename):
'''
:param config:
:type config: Dictionary? defined in dynamic_reconfigure.client.Client
:type nodename: str
'''
#TODO figure out what data type 'config' is. It is afterall returned
# from dynamic_reconfigure.client.get_parameter_descriptions()
# ros.org/doc/api/dynamic_reconfigure/html/dynamic_reconfigure.client-pysrc.html#Client
super(GroupWidget, self).__init__()
self.state = config['state']
self.name = config['name']
self._toplevel_treenode_name = nodename
# TODO: .ui file needs to be back into usage in later phase.
# ui_file = os.path.join(rp.get_path('rqt_reconfigure'),
# 'resource', 'singlenode_parameditor.ui')
# loadUi(ui_file, self)
verticalLayout = QVBoxLayout(self)
verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))
_widget_nodeheader = QWidget()
_h_layout_nodeheader = QHBoxLayout(_widget_nodeheader)
_h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))
self.nodename_qlabel = QLabel(self)
font = QFont('Trebuchet MS, Bold')
font.setUnderline(True)
font.setBold(True)
# Button to close a node.
_icon_disable_node = QIcon.fromTheme('window-close')
_bt_disable_node = QPushButton(_icon_disable_node, '', self)
_bt_disable_node.setToolTip('Hide this node')
_bt_disable_node_size = QSize(36, 24)
_bt_disable_node.setFixedSize(_bt_disable_node_size)
_bt_disable_node.pressed.connect(self._node_disable_bt_clicked)
_h_layout_nodeheader.addWidget(self.nodename_qlabel)
_h_layout_nodeheader.addWidget(_bt_disable_node)
self.nodename_qlabel.setAlignment(Qt.AlignCenter)
font.setPointSize(10)
self.nodename_qlabel.setFont(font)
grid_widget = QWidget(self)
self.grid = QFormLayout(grid_widget)
verticalLayout.addWidget(_widget_nodeheader)
verticalLayout.addWidget(grid_widget, 1)
# Again, these UI operation above needs to happen in .ui file.
self.tab_bar = None # Every group can have one tab bar
self.tab_bar_shown = False
self.updater = updater
self.editor_widgets = []
self._param_names = []
self._create_node_widgets(config)
rospy.logdebug('Groups node name={}'.format(nodename))
self.nodename_qlabel.setText(nodename)
# Labels should not stretch
#self.grid.setColumnStretch(1, 1)
#self.setLayout(self.grid)
def collect_paramnames(self, config):
pass
def _create_node_widgets(self, config):
'''
:type config: Dict?
'''
i_debug = 0
for param in config['parameters']:
begin = time.time() * 1000
editor_type = '(none)'
if param['edit_method']:
widget = EnumEditor(self.updater, param)
elif param['type'] in EDITOR_TYPES:
rospy.logdebug('GroupWidget i_debug=%d param type =%s',
i_debug,
param['type'])
editor_type = EDITOR_TYPES[param['type']]
widget = eval(editor_type)(self.updater, param)
self.editor_widgets.append(widget)
self._param_names.append(param['name'])
rospy.logdebug('groups._create_node_widgets num editors=%d',
i_debug)
end = time.time() * 1000
time_elap = end - begin
rospy.logdebug('ParamG editor={} loop=#{} Time={}msec'.format(
editor_type, i_debug, time_elap))
i_debug += 1
for name, group in config['groups'].items():
if group['type'] == 'tab':
widget = TabGroup(self, self.updater, group)
elif group['type'] in _GROUP_TYPES.keys():
widget = eval(_GROUP_TYPES[group['type']])(self.updater, group)
self.editor_widgets.append(widget)
rospy.logdebug('groups._create_node_widgets ' +
#'num groups=%d' +
'name=%s',
name)
for i, ed in enumerate(self.editor_widgets):
ed.display(self.grid)
rospy.logdebug('GroupWdgt._create_node_widgets len(editor_widgets)=%d',
len(self.editor_widgets))
def display(self, grid, row):
# groups span across all columns
grid.addWidget(self, row, 0, 1, -1)
def update_group(self, config):
self.state = config['state']
# TODO: should use config.keys but this method doesnt exist
names = [name for name in config.items()]
for widget in self.editor_widgets:
if isinstance(widget, EditorWidget):
if widget.name in names:
widget.update_value(config[widget.name])
elif isinstance(widget, GroupWidget):
cfg = find_cfg(config, widget.name)
widget.update_group(cfg)
def close(self):
for w in self.editor_widgets:
w.close()
def get_treenode_names(self):
'''
:rtype: str[]
'''
return self._param_names
def _node_disable_bt_clicked(self):
rospy.logdebug('param_gs _node_disable_bt_clicked')
self.sig_node_disabled_selected.emit(self._toplevel_treenode_name)
class KillCounterPlugin(Plugin):
def __init__(self, context):
super(KillCounterPlugin, self).__init__(context)
#return
# Give QObjects reasonable names
self.setObjectName('KillCounterPlugin')
rp = rospkg.RosPack()
# 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 QWidget
self._widget = QWidget()
self._container = QWidget()
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._num_killed = 0
self._kill_label = QLabel('Martians Killed: 0')
#Killing Martians is Bad Tho, We are a dying species and killing us is mean and will destroy an awesome society with culture and life and happiness. I'm so disappointed in you Kimberly
self._layout.addWidget(self._kill_label)
MIN = 540
MAX = 660
self._chance = randint(MIN, MAX)
context.add_widget(self._container)
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
#ui_file = os.path.join(rp.get_path('rover_ui_plugins'), 'resource', 'CameraSelection.ui')
#ui_file = os.path.join(rp.get_path('rover_ui_plugins'), 'resource', 'CameraSelectionSimple.ui')
# Extend the widget with all attributes and children from UI file
#loadUi(ui_file, self._widget, {})
# Give QObjects reasonable names
self._widget.setObjectName('Kill Counter')
# 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()))
self.running = True
rate = rospy.Rate(1)
def run():
while self.running:
num = randint(0, self._chance)
if num == self._chance:
self._num_killed += 1
self._kill_label.setText('Martians Killed: ' +
str(self._num_killed))
rate.sleep()
self.run_thread = threading.Thread(target=run)
self.run_thread.start()
def shutdown_plugin(self):
# TODO unregister all publishers here`
self.running = False
def __init__(self, context, node_name):
"""
Initializaze things.
:type node_name: str
"""
super(ParamClientWidget, self).__init__()
self._node_grn = node_name
self._toplevel_treenode_name = node_name
self._editor_widgets = {}
self._param_client = create_param_client(
context.node, node_name, self._handle_param_event
)
verticalLayout = QVBoxLayout(self)
verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))
widget_nodeheader = QWidget()
h_layout_nodeheader = QHBoxLayout(widget_nodeheader)
h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))
nodename_qlabel = QLabel(self)
font = QFont('Trebuchet MS, Bold')
font.setUnderline(True)
font.setBold(True)
font.setPointSize(10)
nodename_qlabel.setFont(font)
nodename_qlabel.setAlignment(Qt.AlignCenter)
nodename_qlabel.setText(node_name)
h_layout_nodeheader.addWidget(nodename_qlabel)
# Button to close a node.
icon_disable_node = QIcon.fromTheme('window-close')
bt_disable_node = QPushButton(icon_disable_node, '', self)
bt_disable_node.setToolTip('Hide this node')
bt_disable_node_size = QSize(36, 24)
bt_disable_node.setFixedSize(bt_disable_node_size)
bt_disable_node.pressed.connect(self._node_disable_bt_clicked)
h_layout_nodeheader.addWidget(bt_disable_node)
grid_widget = QWidget(self)
self.grid = QFormLayout(grid_widget)
verticalLayout.addWidget(widget_nodeheader)
verticalLayout.addWidget(grid_widget, 1)
# Again, these UI operation above needs to happen in .ui file.
param_names = self._param_client.list_parameters()
self.add_editor_widgets(
self._param_client.get_parameters(param_names),
self._param_client.describe_parameters(param_names)
)
# Save and load buttons
button_widget = QWidget(self)
button_header = QHBoxLayout(button_widget)
button_header.setContentsMargins(QMargins(0, 0, 0, 0))
load_button = QPushButton()
save_button = QPushButton()
load_button.setIcon(QIcon.fromTheme('document-open'))
save_button.setIcon(QIcon.fromTheme('document-save'))
load_button.clicked[bool].connect(self._handle_load_clicked)
save_button.clicked[bool].connect(self._handle_save_clicked)
button_header.addWidget(save_button)
button_header.addWidget(load_button)
self.setMinimumWidth(150)
def init_sliders(self):
sliderbox = self._widget.findChild(QLayout,'Sliders')
firstCol = QVBoxLayout()
graph_button=QPushButton()
graph_button.setCheckable(True)
graph_button.setText("Graph Off")
graph_button.toggle()
graph_button.clicked.connect(self.set_graph_state)
self.graph_button = graph_button
reset_button=QPushButton()
reset_button.setCheckable(False)
reset_button.setText("Reset")
reset_button.clicked.connect(self.set_reset)
self.graph_button = graph_button
self.reset_button = reset_button
firstCol.addWidget(graph_button)
firstCol.addWidget(reset_button)
firstCol.addWidget(graph_button)
firstCol.setAlignment(graph_button,Qt.AlignVCenter)
zero_button=QPushButton()
zero_button.setCheckable(False)
zero_button.setText("Set All Zero")
zero_button.clicked.connect(self.set_pressure_zero)
self.zero_button = zero_button
firstCol.addWidget(zero_button)
firstCol.setAlignment(zero_button,Qt.AlignVCenter)
transition_box = QVBoxLayout()
label = QLabel()
label.setAlignment(Qt.AlignCenter)
label.setText("Transition Time")
spinbox = QDoubleSpinBox()
spinbox.setMinimum(0)
spinbox.setMaximum(10)
spinbox.setValue(self.settings['transitions'])
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
spinbox.setSuffix(" sec")
spinbox.valueChanged.connect(self.set_transition_value)
transition_box.addWidget(label)
transition_box.addWidget(spinbox)
transition_box.setAlignment(label,Qt.AlignBottom)
transition_box.setAlignment(spinbox,Qt.AlignTop)
firstCol.addLayout(transition_box)
self.sliders = []
sliderbox.addLayout(firstCol)
all_rows_layout = QVBoxLayout()
g_idx = 0
for row in self.settings['gui_config']:
num_groups_row = len(row)
row_layout = QHBoxLayout()
for gr_idx, s_group in enumerate(row):
g_channels = s_group['channels']
g_layout = s_group['layout']
if 'horiz' in g_layout:
group_layout = QHBoxLayout()
else:
group_layout = QVBoxLayout()
control_group = {'sliders': [], 'on_off': None}
label = QLabel()
label.setText("Group. %d"%(g_idx+1))
label.setAlignment(Qt.AlignCenter)
group_layout.addWidget(label)
for c_idx, s_idx in enumerate(g_channels):
idx = s_idx*1
slider_group={'slider':None, 'number':None}
layout_cluster = QVBoxLayout()
labelfirst = QLabel()
labelfirst.setAlignment(Qt.AlignCenter)
labellast = QLabel()
labellast.setAlignment(Qt.AlignCenter)
layout = QVBoxLayout()
if 'diff' in g_layout and c_idx == 0:
sublayout=QHBoxLayout()
layout.setAlignment(Qt.AlignVCenter)
slider = QSlider(Qt.Horizontal)
slider.setMinimum(-100)
slider.setMaximum(100)
slider.setValue(0)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(20)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(-10)
spinbox.setMaximum(10)
spinbox.setValue(0)
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
labellast.setText("%0.1f"%(10)) # These are flipped becasue of order
labelfirst.setText("%0.1f"%(-10))
max_label = labellast
min_label = labelfirst
else:
layout.setAlignment(Qt.AlignHCenter)
slider = QSlider(Qt.Vertical)
slider.setMinimum(self.settings['min_pressure'][idx]*10.0)
slider.setMaximum(self.settings['max_pressure'][idx]*10.0)
slider.setValue(0)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(20)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(self.settings['min_pressure'][idx])
spinbox.setMaximum(self.settings['max_pressure'][idx])
spinbox.setValue(0)
spinbox.setDecimals(1)
spinbox.setSingleStep(0.1)
labelfirst.setText("%0.1f"%(self.settings['max_pressure'][idx]))
labellast.setText("%0.1f"%(self.settings['min_pressure'][idx]))
max_label = labelfirst
min_label = labellast
cb_function_number = lambda value, g_idx=g_idx, s_idx=c_idx, slider=False: self.send_slider_value(g_idx,s_idx,value,slider)
cb_function_slider = lambda value, g_idx=g_idx, s_idx=c_idx, slider=True: self.send_slider_value(g_idx,s_idx,value,slider)
slider.valueChanged.connect(cb_function_slider)
spinbox.valueChanged.connect(cb_function_number)
if 'diff' in g_layout and c_idx == 0:
sublayout.addWidget(labelfirst)
sublayout.addWidget(slider)
sublayout.addWidget(labellast)
layout.addWidget(spinbox)
layout.addLayout(sublayout)
else:
layout.addWidget(labelfirst)
layout.addWidget(slider)
layout.addWidget(labellast)
layout.addWidget(spinbox)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
layout_cluster.addLayout(layout)
slider_group['slider'] = slider
slider_group['number'] = spinbox
slider_group['max_label'] = max_label
slider_group['min_label'] = min_label
control_group['sliders'].append(slider_group)
group_layout.addLayout(layout_cluster)
on_button=QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][idx]:
on_button.toggle()
on_button.setText("On")
on_button.clicked.connect(lambda state, g_idx=g_idx: self.send_channel_state(g_idx,state))
group_layout.addWidget(on_button)
row_layout.addLayout(group_layout)
row_layout.addSpacing(20)
control_group['on_off'] = on_button
self.sliders.append(control_group)
g_idx+=1
all_rows_layout.addLayout(row_layout)
sliderbox.addLayout(all_rows_layout)
class 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 StringLabelWidget(QWidget):
def __init__(self):
super(StringLabelWidget, self).__init__()
self.lock = Lock()
vbox = QVBoxLayout(self)
self.label = QLabel()
self.label.setAlignment(Qt.AlignLeft)
self.label.setSizePolicy(
QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored))
font = QFont("Helvetica", 14)
self.label.setFont(font)
self.label.setWordWrap(True)
vbox.addWidget(self.label)
self.string_sub = None
self._string_topics = []
self._update_topic_timer = QTimer(self)
self._update_topic_timer.timeout.connect(self.updateTopics)
self._update_topic_timer.start(1000)
self._active_topic = None
# to update label visualization
self._dialog = LineEditDialog()
self._rosdata = None
self._start_time = rospy.get_time()
self._update_label_timer = QTimer(self)
self._update_label_timer.timeout.connect(self.updateLabel)
self._update_label_timer.start(40)
def trigger_configuration(self):
self._dialog.exec_()
self.setupSubscriber(self._dialog.value)
def updateLabel(self):
if not self._rosdata:
return
try:
_, data_y = self._rosdata.next()
except rqt_plot.rosplot.RosPlotException as e:
self._rosdata = None
return
if len(data_y) == 0:
return
latest = data_y[-1] # get latest data
# supports std_msgs/String as well as string data nested in rosmsg
if type(latest) == String:
self.string = latest.data
else:
self.string = latest
try:
self.label.setText(self.string)
except TypeError as e:
rospy.logwarn(e)
def updateTopics(self):
need_to_update = False
for (topic, topic_type) in rospy.get_published_topics():
msg = roslib.message.get_message_class(topic_type)
field_names = get_slot_type_field_names(msg, slot_type='string')
for field in field_names:
string_topic = topic + field
if string_topic not in self._string_topics:
self._string_topics.append(string_topic)
need_to_update = True
if need_to_update:
self._string_topics = sorted(self._string_topics)
self._dialog.combo_box.clear()
for topic in self._string_topics:
self._dialog.combo_box.addItem(topic)
if self._active_topic:
if self._active_topic not in self._string_topics:
self._string_topics.append(self._active_topic)
self._dialog.combo_box.addItem(self._active_topic)
self._dialog.combo_box.setCurrentIndex(
self._string_topics.index(self._active_topic))
def setupSubscriber(self, topic):
if not self._rosdata:
self._rosdata = ROSData(topic, self._start_time)
else:
if self._rosdata != topic:
self._rosdata.close()
self._rosdata = ROSData(topic, self._start_time)
else:
rospy.logwarn("%s is already subscribed", topic)
self._active_topic = topic
def onActivated(self, number):
self.setupSubscriber(self._string_topics[number])
def save_settings(self, plugin_settings, instance_settings):
if self._active_topic:
instance_settings.set_value("active_topic", self._active_topic)
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.value("active_topic"):
topic = instance_settings.value("active_topic")
self._dialog.combo_box.addItem(topic)
self.setupSubscriber(topic)
def __init__(self, node_ns, controller, urdf=None, *args, **kwargs):
super(JointPositionControllerWidget, self).__init__(*args, **kwargs)
self._controller = controller
self._urdf = urdf
loadUi('Controller.ui', self)
self.controller_name.setText(controller.name)
self.controller_type.setText(controller.type)
controller_ns = rospy.resolve_name(controller.name, node_ns)
joint_name_widget = QLabel()
joint_position_widget = QLineEdit()
joint_position_widget.setReadOnly(True)
joint_position_widget.setAlignment(Qt.AlignRight)
joint_position_widget.setText(str(0.0))
extras_layout = QGridLayout()
extras_layout.addWidget(joint_name_widget, 0, 0)
extras_layout.addWidget(joint_position_widget, 0, 1)
extras = QWidget()
extras.setLayout(extras_layout)
self.layout().addWidget(extras)
slider_range = (0, 10000)
limits = {'min': 0.0, 'max': 3.14}
if self._urdf is not None:
joint_name = next(iter([
resource
for interface in controller.claimed_resources
if interface.hardware_interface == \
'hardware_interface::PositionJointInterface'
for resource in interface.resources
]), None)
if joint_name is not None:
joint_name_widget.setText(joint_name)
joint_limit = self._urdf.xpath(
'/robot/joint[@name=$name]/limit', name=joint_name)
if joint_limit:
l = joint_limit[0]
limits = {
'min': float(joint_limit[0].get('lower')),
'max': float(joint_limit[0].get('upper'))
}
pub = rospy.Publisher(controller_ns + '/command',
std_msgs.msg.Float64,
queue_size=10)
def on_value_changed(value):
value = limits['min'] + value / float(
slider_range[1] - slider_range[0]) * (limits['max'] -
limits['min'])
joint_position_widget.setText(str(value))
pub.publish(value)
slider = QSlider(Qt.Horizontal)
slider.setRange(slider_range[0], slider_range[1])
slider.valueChanged.connect(on_value_changed)
extras_layout.addWidget(slider, 1, 0, 1, 2)
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 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_sliders(self):
sliderbox = self._widget.findChild(QLayout, 'Sliders')
graph_button = QPushButton()
graph_button.setCheckable(True)
graph_button.setText("Graph Off")
graph_button.toggle()
graph_button.clicked.connect(self.set_graph_state)
self.graph_button = graph_button
firstCol = QVBoxLayout()
firstCol.addWidget(graph_button)
sliderbox.addLayout(firstCol)
self.sliders = []
all_rows_layout = QVBoxLayout()
chan_idx = 0
for num_channels_row in self.settings['num_channels']:
row_layout = QHBoxLayout()
for i in range(num_channels_row):
idx = chan_idx * 1
slider_group = {
'slider_p': None,
'number_p': None,
'slider_i': None,
'number_i': None,
'slider_d': None,
'number_d': None,
'on_off': None
}
layout_cluster = QVBoxLayout()
slider_cluster = QHBoxLayout()
label = QLabel()
label.setText("Chan. %d" % (idx + 1))
label.setAlignment(Qt.AlignCenter)
layout_cluster.addWidget(label)
for j in range(3):
layout = QVBoxLayout()
layout.setAlignment(Qt.AlignHCenter)
if j == 0:
maxrange = 1.0
elif j == 1:
maxrange = 10
elif j == 2:
maxrange = 10
slider = QSlider(Qt.Vertical)
slider.setMinimum(0)
slider.setMaximum(maxrange * 100)
slider.setValue(self.settings['pid_gains'][chan_idx][j] *
100)
slider.setTickPosition(QSlider.TicksRight)
slider.setTickInterval(maxrange / 100.0)
spinbox = QDoubleSpinBox()
spinbox.setMinimum(0)
spinbox.setMaximum(maxrange)
spinbox.setValue(self.settings['pid_gains'][chan_idx][j])
spinbox.setDecimals(2)
spinbox.setSingleStep(maxrange / 100.0)
cb_function_number = lambda value, idx=idx, gain_idx=j, slider=False: self.send_slider_value(
idx, gain_idx, value, slider)
cb_function_slider = lambda value, idx=idx, gain_idx=j, slider=True: self.send_slider_value(
idx, gain_idx, value, slider)
slider.valueChanged.connect(cb_function_slider)
spinbox.valueChanged.connect(cb_function_number)
label = QLabel()
label.setAlignment(Qt.AlignCenter)
if j == 0:
slider_group['slider_p'] = slider
slider_group['number_p'] = spinbox
label.setText("P")
elif j == 1:
slider_group['slider_i'] = slider
slider_group['number_i'] = spinbox
label.setText("I")
elif j == 2:
slider_group['slider_d'] = slider
slider_group['number_d'] = spinbox
label.setText("D")
labelmax = QLabel()
labelmax.setAlignment(Qt.AlignCenter)
labelmax.setText("%0.1f" % (maxrange))
labelmin = QLabel()
labelmin.setAlignment(Qt.AlignCenter)
labelmin.setText("0")
layout.addWidget(label)
layout.addWidget(labelmax)
layout.addWidget(slider, Qt.AlignHCenter)
layout.addWidget(labelmin)
layout.addWidget(spinbox, Qt.AlignHCenter)
layout.setAlignment(slider, Qt.AlignHCenter)
layout.setAlignment(spinbox, Qt.AlignHCenter)
slider_cluster.addLayout(layout)
on_button = QPushButton()
on_button.setCheckable(True)
on_button.setText("Off")
if self.settings['channel_states'][chan_idx]:
on_button.toggle()
on_button.setText("On")
on_button.clicked.connect(
lambda state, idx=idx: self.send_channel_state(idx, state))
slider_group['on_off'] = on_button
layout_cluster.addLayout(slider_cluster)
layout_cluster.addWidget(on_button)
row_layout.addLayout(layout_cluster)
row_layout.addSpacing(20)
self.sliders.append(slider_group)
chan_idx += 1
all_rows_layout.addLayout(row_layout)
sliderbox.addLayout(all_rows_layout)
class GroupWidget(QWidget):
"""
(Isaac's guess as of 12/13/2012)
This class bonds multiple Editor instances that are associated with
a single node as a group.
"""
# public signal
sig_node_disabled_selected = Signal(str)
sig_node_state_change = Signal(bool)
def __init__(self, updater, config, nodename):
"""
:param config:
:type config: Dictionary? defined in dynamic_reconfigure.client.Client
:type nodename: str
"""
super(GroupWidget, self).__init__()
self.state = config['state']
self.param_name = config['name']
self._toplevel_treenode_name = nodename
# TODO: .ui file needs to be back into usage in later phase.
# ui_file = os.path.join(rp.get_path('rqt_reconfigure'),
# 'resource', 'singlenode_parameditor.ui')
# loadUi(ui_file, self)
verticalLayout = QVBoxLayout(self)
verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))
_widget_nodeheader = QWidget()
_h_layout_nodeheader = QHBoxLayout(_widget_nodeheader)
_h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))
self.nodename_qlabel = QLabel(self)
font = QFont('Trebuchet MS, Bold')
font.setUnderline(True)
font.setBold(True)
# Button to close a node.
_icon_disable_node = QIcon.fromTheme('window-close')
_bt_disable_node = QPushButton(_icon_disable_node, '', self)
_bt_disable_node.setToolTip('Hide this node')
_bt_disable_node_size = QSize(36, 24)
_bt_disable_node.setFixedSize(_bt_disable_node_size)
_bt_disable_node.pressed.connect(self._node_disable_bt_clicked)
_h_layout_nodeheader.addWidget(self.nodename_qlabel)
_h_layout_nodeheader.addWidget(_bt_disable_node)
self.nodename_qlabel.setAlignment(Qt.AlignCenter)
font.setPointSize(10)
self.nodename_qlabel.setFont(font)
grid_widget = QWidget(self)
self.grid = QFormLayout(grid_widget)
verticalLayout.addWidget(_widget_nodeheader)
verticalLayout.addWidget(grid_widget, 1)
# Again, these UI operation above needs to happen in .ui file.
self.tab_bar = None # Every group can have one tab bar
self.tab_bar_shown = False
self.updater = updater
self.editor_widgets = []
self._param_names = []
self._create_node_widgets(config)
logging.debug('Groups node name={}'.format(nodename))
self.nodename_qlabel.setText(nodename)
# Labels should not stretch
# self.grid.setColumnStretch(1, 1)
# self.setLayout(self.grid)
def collect_paramnames(self, config):
pass
def _create_node_widgets(self, config):
"""
:type config: Dict?
"""
i_debug = 0
for param in config['parameters']:
begin = time.time() * 1000
editor_type = '(none)'
if param['edit_method']:
widget = EnumEditor(self.updater, param)
elif param['type'] in EDITOR_TYPES:
logging.debug('GroupWidget i_debug={} param type ={}'.format(
i_debug, param['type']))
editor_type = EDITOR_TYPES[param['type']]
widget = eval(editor_type)(self.updater, param)
self.editor_widgets.append(widget)
self._param_names.append(param['name'])
logging.debug(
'groups._create_node_widgets num editors={}'.format(i_debug))
end = time.time() * 1000
time_elap = end - begin
logging.debug('ParamG editor={} loop=#{} Time={}msec'.format(
editor_type, i_debug, time_elap))
i_debug += 1
for name, group in sorted(config['groups'].items()):
if group['type'] == 'tab':
widget = TabGroup(self, self.updater, group,
self._toplevel_treenode_name)
elif group['type'] in _GROUP_TYPES.keys():
widget = eval(_GROUP_TYPES[group['type']])(
self.updater, group, self._toplevel_treenode_name)
else:
widget = eval(_GROUP_TYPES[''])(self.updater, group,
self._toplevel_treenode_name)
self.editor_widgets.append(widget)
logging.debug('groups._create_node_widgets name={}'.format(name))
for i, ed in enumerate(self.editor_widgets):
ed.display(self.grid)
logging.debug('GroupWdgt._create_node_widgets'
' len(editor_widgets)={}'.format(len(
self.editor_widgets)))
def display(self, grid):
grid.addRow(self)
def update_group(self, config):
if 'state' in config:
old_state = self.state
self.state = config['state']
if self.state != old_state:
self.sig_node_state_change.emit(self.state)
names = [name for name in config.keys()]
for widget in self.editor_widgets:
if isinstance(widget, EditorWidget):
if widget.param_name in names:
widget.update_value(config[widget.param_name])
elif isinstance(widget, GroupWidget):
cfg = find_cfg(config, widget.param_name) or config
widget.update_group(cfg)
def close(self):
for w in self.editor_widgets:
w.close()
def get_treenode_names(self):
"""
:rtype: str[]
"""
return self._param_names
def _node_disable_bt_clicked(self):
logging.debug('param_gs _node_disable_bt_clicked')
self.sig_node_disabled_selected.emit(self._toplevel_treenode_name)
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)