class ReadWriteParamsWorker(QtCore.QObject):
ret = QtCore.pyqtSignal(object)
finished = QtCore.pyqtSignal()
progress = QtCore.pyqtSignal(int, int)
def __init__(self, fun, param_names=None, param_values=None):
super(ReadWriteParamsWorker, self).__init__()
self.names = param_names
self.values = param_values
self.fun = fun
def set_params(self, names, values=None):
self.names = names
self.values = values
def run(self):
ret_values = dict()
if self.names is not None:
self.progress.emit(0, len(self.names))
for i, param in enumerate(self.names):
if self.values is None:
ret_values[param] = self.fun(param)
else:
ret_values[param] = self.fun(param, self.values[i])
self.progress.emit(i + 1, len(self.names))
self.ret.emit(ret_values)
self.finished.emit()
class UploadWorker(QtCore.QObject):
ret = QtCore.pyqtSignal(object)
finished = QtCore.pyqtSignal()
program_progress = QtCore.pyqtSignal(int, int)
verify_progress = QtCore.pyqtSignal(int, int)
def __init__(self, fun=None, path=None):
super(UploadWorker, self).__init__()
self.upload = fun
self.path = path
def set_fun(self, fun):
self.upload = fun
def set_path(self, path):
self.path = path
def on_program_progress(self, progress, total):
self.program_progress.emit(progress, total)
def on_verify_progress(self, progress, total):
self.verify_progress.emit(progress, total)
def run(self):
ret = None
try:
ret = self.upload(self.path)
except Exception as e:
logger.error("Error during firmware upload: %s", e)
self.ret.emit(ret)
self.finished.emit()
class Worker(QtCore.QObject):
ret = QtCore.pyqtSignal(object)
finished = QtCore.pyqtSignal()
def __init__(self, fun, *args, **kwargs):
super(Worker, self).__init__()
self.fun = fun
self.args = args
self.kwargs = kwargs
def run(self):
self.ret.emit(self.fun(*self.args, **self.kwargs))
self.finished.emit()
class Channels(QtCore.QObject):
vehicle_create = QtCore.pyqtSignal(str, str, dict, list)
vehicle_move = QtCore.pyqtSignal(str, dict)
vehicle_hide = QtCore.pyqtSignal(str)
vehicle_show = QtCore.pyqtSignal(str)
rqt_vviz_resize = QtCore.pyqtSignal(list)
rqt_vviz_clear = QtCore.pyqtSignal()
road_marking_set_size = QtCore.pyqtSignal(list, int)
road_marking_edit_first = QtCore.pyqtSignal(str, int)
draw_circle = QtCore.pyqtSignal(str, dict, int, str)
remove_circle = QtCore.pyqtSignal(str)
class QtParam(QtGui.QWidget):
accepted = QtCore.pyqtSignal(list)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
path = rospkg.RosPack().get_path('cyplam_param')
loadUi(os.path.join(path, 'resources', 'param.ui'), self)
self.btnAccept.clicked.connect(self.btnAcceptClicked)
if rospy.has_param('/powder'):
self.setPowderParameters(rospy.get_param('/powder'))
if rospy.has_param('/process'):
self.setProcessParameters(rospy.get_param('/process'))
def getPowderParameters(self):
params = {
'shield': self.sbShield.value(),
'carrier': self.sbCarrier.value(),
'stirrer': self.sbStirrer.value(),
'turntable': self.sbTurntable.value()
}
return params
def setPowderParameters(self, params):
self.sbShield.setValue(params['shield'])
self.sbCarrier.setValue(params['carrier'])
self.sbStirrer.setValue(params['stirrer'])
self.sbTurntable.setValue(params['turntable'])
def getProcessParameters(self):
params = {
'speed': self.sbSpeed.value(),
'power': self.sbPower.value(),
'focus': self.sbFocus.value()
}
return params
def setProcessParameters(self, params):
self.sbSpeed.setValue(params['speed'])
self.sbPower.setValue(params['power'])
self.sbFocus.setValue(params['focus'])
def btnAcceptClicked(self):
print '# Powder parameters'
powder = self.getPowderParameters()
rospy.set_param('/powder', powder)
print 'Powder parameters:', rospy.get_param('/powder')
print '# Process parameters'
process = self.getProcessParameters()
rospy.set_param('/process', process)
print 'Process parameters:', rospy.get_param('/process')
self.accepted.emit([])
class SingleApplication(QtGui.QApplication):
messageAvailable = QtCore.pyqtSignal(object)
def __init__(self, argv, key):
QtGui.QApplication.__init__(self, argv)
self._memory = QtCore.QSharedMemory(self)
self._memory.setKey(key)
if self._memory.attach():
self._running = True
else:
self._running = False
if not self._memory.create(1):
raise RuntimeError(self._memory.errorString())
def isRunning(self):
return self._running
class ConMonGui(QtGui.QWidget):
onSendState = QtCore.pyqtSignal(object, int, name='onSendState')
def __init__(self):
#Init the base class
QtGui.QWidget.__init__(self)
self.parents = dict();
def setup(self, name, ros):
self.setObjectName(name)
self.setWindowTitle(name)
self._setup();
self._connect_signals(ros)
@QtCore.pyqtSlot(object)
def onNewController(self, req):
if not self.parents.has_key(req.parent):
self.parents[req.parent] = QtGui.QTreeWidgetItem()
self.parents[req.parent].setText(0, req.parent)
self.controllerTree.addTopLevelItem(self.parents[req.parent])
item = QtGui.QTreeWidgetItem()
item.setText(0, req.name)
self.parents[req.parent].addChild(item)
@QtCore.pyqtSlot()
def onSend(self):
self.onSendState.emit(self.nameEdit.text(), self.stateCombo.currentIndex())
def unload(self):
pass
def _connect_signals(self, ros):
self.sendButton.clicked.connect(self.onSend)
self.onSendState.connect(ros.onNewState)
pass
def _setup(self):
self.stateCombo.addItem("DISABLED")
self.stateCombo.addItem("MANUAL")
self.stateCombo.addItem("EXTERNAL")
self.stateCombo.addItem("TRACKING")
pass
class QtParam(QtGui.QWidget):
accepted = QtCore.pyqtSignal(list)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
loadUi(os.path.join(path, 'resources', 'param.ui'), self)
# self.sbShield.valueChanged.connect(self.changeShield)
# self.sbCarrier.valueChanged.connect(self.changeCarrier)
# self.sbStirrer.valueChanged.connect(self.changeStirrer)
# self.sbTurntable.valueChanged.connect(self.changeTurntable)
self.sbSpeed.valueChanged.connect(self.changeSpeed)
self.sbPower.valueChanged.connect(self.changePower)
self.btnAccept.clicked.connect(self.btnAcceptClicked)
def updateSpeed(self, data):
rospy.loginfo(rospy.get_caller_id() + " Speed: %s ", data.speed)
self.lblInfo.setText('Transverse speed: %.3f m/s' % data.speed)
# TODO: Add event to notify an speed change
def changeSpeed(self):
speed = self.sbSpeed.value()
#self.robpath.set_speed(speed)
print 'Speed:', speed
def changePower(self):
power = self.sbPower.value()
#self.robpath.set_power(power)
print 'Power:', power
def btnAcceptClicked(self):
print 'Shield:', self.sbShield.value()
print 'Carrier:', self.sbCarrier.value()
print 'Stirrer:', self.sbStirrer.value()
print 'Turntable:', self.sbTurntable.value()
spd = self.sbSpeed.value()
pwr = self.sbPower.value()
params = ['{"laser_prog": 11}',
'{"laser_pow": %i}' % int((pwr * 65535) / 1500),
'{"vel": %i}' % spd]
self.accepted.emit(params)
class SingleApplication(QtGui.QApplication):
messageAvailable = QtCore.pyqtSignal(object)
def __init__(self, argv, key):
QtGui.QApplication.__init__(self, argv)
self._memory = QtCore.QSharedMemory(self)
self._memory.setKey(key)
if self._memory.attach():
self._running = True
else:
self._running = False
if not self._memory.create(1):
raise RuntimeError(self._memory.errorString())
# Set correct logo
base_path = os.path.abspath(os.path.dirname(__file__))
icon_path = os.path.join(base_path, 'images', 'hyperspy.svg')
QtGui.QApplication.setWindowIcon(QtGui.QIcon(icon_path))
def isRunning(self):
return self._running
class ConMonROS(QtCore.QObject):
onNewController = QtCore.pyqtSignal(object, name = "onNewController")
def __init__(self):
QtCore.QObject.__init__(self)
def setup(self, gui):
self._connect_signals(gui)
self._subscribe()
pass
def unload(self):
pass
def _connect_signals(self, gui):
self.onNewController.connect(gui.onNewController)
pass
def onRegisterController(self, req):
self.onNewController.emit(req)
return RegisterController_v3Response()
@QtCore.pyqtSlot(object, int)
def onNewState(self, name, state):
out = ControllerState()
out.name = [name];
info = ControllerInfo()
info.state = state
out.info = [info]
self.stateOut.publish(out)
def _subscribe(self):
self.srv = rospy.Service('register_controller',
RegisterController_v3,
self.onRegisterController)
self.stateOut = rospy.Publisher('controller_state',ControllerState)
def _unsubscribe(self):
pass
class VoltageWidget(QWidget):
resized = QtCore.pyqtSignal()
def __init__(self):
super(VoltageWidget, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('voltage_gui'), 'resource',
'voltage_gui.ui')
loadUi(ui_file, self)
# Whenever the screen is resized the resizeFont function is called
self.resized.connect(self.resizeFont)
self.height = VoltageWidget.frameGeometry(self).height()
self.fontSize = 40
self.warningCounter = 0
self.paramCounter = 0
self.initThresh()
# Subscribing to all the data we need
self.battery_voltage = None
rospy.Subscriber("/battery_monitor", Float32, self.updateMain)
rospy.Subscriber("/FL_motor/feedback", Feedback, self.update_FL)
self.voltageFL = None
rospy.Subscriber("/FR_motor/feedback", Feedback, self.update_FR)
self.voltageFR = None
rospy.Subscriber("/BL_motor/feedback", Feedback, self.update_BL)
self.voltageBL = None
rospy.Subscriber("/BR_motor/feedback", Feedback, self.update_BR)
self.voltageBR = None
# The functions that the subscribers call in order to get new data
def updateMain(self, mainData):
self.battery_voltage = mainData
def update_FL(self, dataFL):
self.voltageFL = dataFL.supply_voltage
def update_FR(self, dataFR):
self.voltageFR = dataFR.supply_voltage
def update_BL(self, dataBL):
self.voltageBL = dataBL.supply_voltage
def update_BR(self, dataBR):
self.voltageBR = dataBR.supply_voltage
# Part of signal that notifies program whenever window is resized
def resizeEvent(self, event):
self.resized.emit()
return super(VoltageWidget, self).resizeEvent(event)
# Increase/decrease size of fonts based on window resize
def resizeFont(self):
# gets new window dimensions, the self is needed because we are referencing
# our VoltageWidget class
height = VoltageWidget.frameGeometry(self).height()
# The ratio change in width and height caused by the resize
heightRatio = height / self.height
# update to current
self.height = height
# Fonts like 16, 24 are references to height of letters. So I thought it would
# Make sense to change font size proportionally to changes in window height
self.fontSize = self.fontSize * heightRatio
newfont = QtGui.QFont("Times", self.fontSize, QtGui.QFont.Bold)
self.labelMain.setFont(newfont)
self.labelFL.setFont(newfont)
self.labelFR.setFont(newfont)
self.labelBL.setFont(newfont)
self.labelBR.setFont(newfont)
threshFont = QtGui.QFont("Times", (self.fontSize) / 3,
QtGui.QFont.Bold)
self.labelThresh.setFont(threshFont)
# Sets the text of the thrshold info box
def initThresh(self):
# Low and Critical decide what colors the boxes take for
# Good (Green), Warning (Yellow), and Critical (Red)
# If the parameter server has not set these values then we use the DEFAULT
# values as of Oct. 2017 which are 26 and 20
# The GUI notifies that it is using defualt values and will continually
# check to see if the params have been set, as symboloized by self.gotParams
self.gotParams = True
self.lowThreshold = 26
self.criticalThreshold = 20
try:
self.lowThreshold = rospy.get_param('battery-voltage/low')
self.criticalThreshold = rospy.get_param(
'battery-voltage/critical')
except:
print("Low and Critical Voltage not set, Low: 26 Critical:20")
self.lowThreshold = 26
self.criticalThreshold = 20
self.gotParams = False
# Thresh is a box in the top right of the GUI that displays Threshold values and box layouts
# self.labelThresh = QLabel(self)
# self.labelThresh.setGeometry(QtCore.QRect((150+2*self.boxWidth), (10), self.boxWidth, self.boxHeight))
threshText = "Low Threshold: {} \nCritical: {}".format(
self.lowThreshold, self.criticalThreshold)
self.labelThresh.setText(threshText)
self.labelThresh.setStyleSheet(
"QLabel { background-color : white; color : black; }")
threshFont = QtGui.QFont("Times", (self.fontSize) / 3,
QtGui.QFont.Bold)
self.labelThresh.setFont(threshFont)
# If self.gotParams is False, the updateLabel function calls testParams every 5 seconds
def testParams(self):
try:
self.lowThreshold = rospy.get_param('battery-voltage/low')
self.criticalThreshold = rospy.get_param(
'battery-voltage/critical')
self.gotParams = True
except:
self.gotParams = False
if self.gotParams:
threshText = "Low Threshold: {} \nCritical: {}".format(
self.lowThreshold, self.criticalThreshold)
else:
threshText = "THRESHOLDS NOT SET\nUSING DEFAULT\nLow Threshold: {} \nCritical: {}".format(
self.lowThreshold, self.criticalThreshold)
self.labelThresh.setText(threshText)
# sets colors of boxes based on current values of voltages for each box
def setColors(self, numMain):
if numMain > self.lowThreshold:
self.labelMain.setStyleSheet(
"QLabel { background-color : green; color : white; }")
elif numMain <= self.lowThreshold and numMain > self.criticalThreshold:
self.labelMain.setStyleSheet(
"QLabel { background-color : yellow; color : black; }")
elif numMain <= self.criticalThreshold:
self.labelMain.setStyleSheet(
"QLabel { background-color : red; color : white; }")
if self.voltageFL > self.lowThreshold:
self.labelFL.setStyleSheet(
"QLabel { background-color : green; color : white; }")
elif self.voltageFL <= self.lowThreshold and self.voltageFL > self.criticalThreshold:
self.labelFL.setStyleSheet(
"QLabel { background-color : yellow; color : black; }")
elif self.voltageFL <= self.criticalThreshold:
self.labelFL.setStyleSheet(
"QLabel { background-color : red; color : white; }")
if self.voltageFR > self.lowThreshold:
self.labelFR.setStyleSheet(
"QLabel { background-color : green; color : white; }")
elif self.voltageFR <= self.lowThreshold and self.voltageFR > self.criticalThreshold:
self.labelFR.setStyleSheet(
"QLabel { background-color : yellow; color : black; }")
elif self.voltageFR <= self.criticalThreshold:
self.labelFR.setStyleSheet(
"QLabel { background-color : red; color : white; }")
if self.voltageBL > self.lowThreshold:
self.labelBL.setStyleSheet(
"QLabel { background-color : green; color : white; }")
elif self.voltageBL <= self.lowThreshold and self.voltageBL > self.criticalThreshold:
self.labelBL.setStyleSheet(
"QLabel { background-color : yellow; color : black; }")
elif self.voltageBL <= self.criticalThreshold:
self.labelBL.setStyleSheet(
"QLabel { background-color : red; color : white; }")
if self.voltageBR > self.lowThreshold:
self.labelBR.setStyleSheet(
"QLabel { background-color : green; color : white; }")
elif self.voltageBR <= self.lowThreshold and self.voltageBR > self.criticalThreshold:
self.labelBR.setStyleSheet(
"QLabel { background-color : yellow; color : black; }")
elif self.voltageBR <= self.criticalThreshold:
self.labelBR.setStyleSheet(
"QLabel { background-color : red; color : white; }")
def updateLabel(self):
# Tries self.gotParams every 3 function calls
self.paramCounter = self.paramCounter + 1
if self.gotParams is False and self.paramCounter >= 3:
self.testParams()
self.paramCounter = 0
# Checks if battery_monitor.py is online, if not it sets that voltage to 0
# otherwise ite formats the voltage data into an int to be shown in GUI
try:
stringMain = str(self.battery_voltage)
stringMain = stringMain[5:]
numMain = float(stringMain)
numMain = int(numMain * 100)
numMain = numMain / 100.0
except:
if self.warningCounter == 0:
print("battery monitor is not online!!")
self.warningCounter = 1
numMain = 0.0
self.setColors(numMain)
# Turns all the voltages into strings and sets them as text in GUI boxes
self.labelMain.setText("{}".format(numMain))
try:
numFL = (int(self.voltageFL * 100)) / 100
stringFL = str(numFL)
self.labelFL.setText("{}".format(stringFL))
except:
pass
try:
numFR = (int(self.voltageFR * 100)) / 100
stringFR = str(numFR)
self.labelFR.setText("{}".format(stringFR))
except:
pass
try:
numBL = (int(self.voltageBL * 100)) / 100
stringBL = str(numBL)
self.labelBL.setText("{}".format(stringBL))
except:
pass
try:
stringBR = float("{0:.2f}".format(self.voltageBR))
self.labelBR.setText("{}".format(stringBR))
except:
pass
QApplication.processEvents()
class ComboBox(QtWidgets.QComboBox):
popup = QtCore.pyqtSignal()
def showPopup(self):
self.popup.emit()
super(ComboBox, self).showPopup()
class RadioConfiguratorWidget(QtWidgets.QWidget):
BAUDS = [2400, 4800, 9600, 19200, 38400, 57600, 115200]
BAUD_DEFAULT = 57600
connected = QtCore.pyqtSignal()
disconnected = QtCore.pyqtSignal()
at_entered = QtCore.pyqtSignal()
at_exited = QtCore.pyqtSignal()
def __init__(self):
super(RadioConfiguratorWidget, self).__init__()
self.configurator = radio_tools.Configurator()
self._is_connected = False
self._in_at_mode = False
self._enter_at_thread = None
self._enter_at_worker = None
self._read_params_thread = None
self._read_params_worker = None
self.setup_background_threads()
ui_file = os.path.join(rospkg.RosPack().get_path("radio_tools"),
"resource", "radio_configurator.ui")
loadUi(ui_file, self)
self.at_entered.connect(self.on_at_entered)
self.at_exited.connect(self.on_at_exited)
self.logging_handler = QTextEditLogger(self.console_text)
self.logging_handler.setFormatter(
logging.Formatter("%(asctime)s - %(levelname)s - %(message)s"))
self.logger = logging.getLogger("radio_gui")
self.logger.addHandler(self.logging_handler)
self.logger.setLevel(logging.DEBUG)
self.init_logging_radiobuttons()
self.connected.connect(self.on_connected)
self.disconnected.connect(self.on_disconnected)
self.init_connection_group()
self.eeprom_group.setEnabled(False)
self.load_settings_progressbar.reset()
self.init_at_group()
self.init_param_widgets()
def init_logging_radiobuttons(self):
def connect_button_toggled(button):
button.toggled.connect(
lambda: self.handle_logging_radiobutton(button))
buttons = self.logger_level_group.findChildren(QtWidgets.QRadioButton)
for button in buttons:
connect_button_toggled(button)
self.info_radiobutton.setChecked(True)
def handle_logging_radiobutton(self, button):
if not button.isChecked():
return
if button.text() == "Debug":
self.logger.setLevel(logging.DEBUG)
self.logger.debug("Setting debug level to %s.",
button.text().upper())
elif button.text() == "Info":
self.logger.debug("Setting debug level to %s.",
button.text().upper())
self.logger.setLevel(logging.INFO)
elif button.text() == "Warn":
self.logger.debug("Setting debug level to %s.",
button.text().upper())
self.logger.setLevel(logging.WARN)
elif button.text() == "Error":
self.logger.debug("Setting debug level to %s.",
button.text().upper())
self.logger.setLevel(logging.ERROR)
else:
self.logger.error("Unhandled logging level: %s.", button.text())
def setup_background_threads(self):
def connect_worker_and_thread(worker, thread, return_cb, finished_cb):
worker.moveToThread(thread)
thread.started.connect(worker.run)
worker.ret.connect(return_cb)
worker.finished.connect(finished_cb)
self._enter_at_thread = QtCore.QThread()
self._enter_at_worker = Worker(self.configurator.enter_at_mode)
connect_worker_and_thread(worker=self._enter_at_worker,
thread=self._enter_at_thread,
return_cb=self.on_enter_at_result,
finished_cb=self.on_enter_at_finished)
self._read_params_thread = QtCore.QThread()
self._read_params_worker = ReadWriteParamsWorker(
self.configurator.read_param)
connect_worker_and_thread(worker=self._read_params_worker,
thread=self._read_params_thread,
return_cb=self.on_read_params_result,
finished_cb=self.on_read_params_finished)
self._read_params_worker.progress.connect(self.update_load_progressbar)
self._write_params_thread = QtCore.QThread()
self._write_params_worker = ReadWriteParamsWorker(
self.configurator.write_param)
connect_worker_and_thread(worker=self._write_params_worker,
thread=self._write_params_thread,
return_cb=self.on_write_params_result,
finished_cb=self.on_write_params_finished)
self._write_params_worker.progress.connect(
self.update_copy_settings_progressbar)
self._upload_firmware_thread = QtCore.QThread()
self._upload_firmware_worker = UploadWorker()
connect_worker_and_thread(worker=self._upload_firmware_worker,
thread=self._upload_firmware_thread,
return_cb=self.on_upload_firmware_result,
finished_cb=self.on_upload_firmware_finished)
self._upload_firmware_worker.set_fun(
lambda path: self.configurator.upload_firmware(
path, self._upload_firmware_worker.on_program_progress, self.
_upload_firmware_worker.on_verify_progress))
self._upload_firmware_worker.program_progress.connect(
self.on_program_progress)
self._upload_firmware_worker.verify_progress.connect(
self.on_verify_progress)
def on_enter_at_finished(self):
self._enter_at_thread.quit()
self._enter_at_thread.wait()
def on_read_params_finished(self):
self._read_params_thread.quit()
self._read_params_thread.wait()
def on_write_params_finished(self):
self._write_params_thread.quit()
self._write_params_thread.wait()
def on_upload_firmware_finished(self):
self._upload_firmware_thread.quit()
self._upload_firmware_thread.wait()
self.upload_button.setEnabled(True)
@QtCore.pyqtSlot(object)
def on_upload_firmware_result(self, result):
if not result:
txt = "Failed to upload/verify firmware!"
self.logger.error(txt)
show_error_box(txt)
else:
txt = "Firmware upload completed successfully."
self.logger.info(txt)
show_message_box(txt)
def init_connection_group(self):
self.port_combobox.popup.connect(self.update_port_combobox)
self.baud_combobox.addItems([str(baud) for baud in self.BAUDS])
for i in range(len(self.BAUDS)):
if str(self.BAUD_DEFAULT) == self.baud_combobox.itemText(i):
self.baud_combobox.setCurrentIndex(i)
def init_at_group(self):
self.at_group.setEnabled(False)
enable_all_buttons_in_group(self.at_group, False)
self.enter_at_button.setEnabled(True)
def update_port_combobox(self):
ports = scan_ports()
current_text = self.port_combobox.currentText()
self.port_combobox.clear()
self.port_combobox.addItems(ports)
if current_text in ports:
for i in range(len(ports)):
if current_text == self.port_combobox.itemText(i):
self.port_combobox.setCurrentIndex(i)
break
@QtCore.pyqtSlot(int, int)
def on_program_progress(self, progress, total):
self.program_progressbar.setMinimum(0)
self.program_progressbar.setMaximum(total)
self.program_progressbar.setValue(progress)
@QtCore.pyqtSlot(int, int)
def on_verify_progress(self, progress, total):
self.verify_progressbar.setMinimum(0)
self.verify_progressbar.setMaximum(total)
self.verify_progressbar.setValue(progress)
@QtCore.pyqtSlot()
def on_connected(self):
self.logger.info("Connected to '%s@%s'", self.configurator.port.port,
self.configurator.port.baudrate)
self.connect_button.setEnabled(False)
self.disconnect_button.setEnabled(True)
self.eeprom_group.setEnabled(True)
self.firmware_group.setEnabled(True)
self.load_settings_button.setEnabled(False)
self.copy_settings_button.setEnabled(False)
self.at_group.setEnabled(True)
self.connection_status_label.setText("connected")
self.do_enter_at_mode()
@QtCore.pyqtSlot()
def on_connect_button_clicked(self):
port = self.port_combobox.currentText()
baud = int(self.baud_combobox.currentText())
self.configurator.port.port = port
self.configurator.port.baudrate = baud
try:
if not self.configurator.port.is_open:
self.configurator.port.open()
except Exception as e:
show_error_box(e)
self.eeprom_group.setEnabled(False)
self.connection_status_label.setText("failed to connect")
else:
self.connected.emit()
@QtCore.pyqtSlot()
def on_disconnected(self):
self.disconnect_button.setEnabled(False)
self.connect_button.setEnabled(True)
self.connection_status_label.setText("disconnected")
self.at_group.setEnabled(False)
self.eeprom_group.setEnabled(False)
self.firmware_group.setEnabled(False)
self.load_settings_button.setEnabled(False)
self.copy_settings_button.setEnabled(False)
@QtCore.pyqtSlot()
def on_disconnect_button_clicked(self):
self.do_disconnect()
def do_disconnect(self):
self.configurator.exit_at_mode()
self.configurator.port.flush()
self.configurator.port.close()
self.disconnected.emit()
def do_enter_at_mode(self):
if self._enter_at_thread.isRunning():
show_error_box("You are already trying to enter AT mode.")
else:
self.enter_at_button.setEnabled(False)
self.logger.info("Entering AT mode...")
self.at_mode_progressbar_connecting()
self._enter_at_thread.start()
@QtCore.pyqtSlot()
def on_enter_at_button_clicked(self):
self.do_enter_at_mode()
@QtCore.pyqtSlot()
def on_at_entered(self):
self.at_mode_progressbar_connected()
enable_all_buttons_in_group(self.at_group, True)
self.enter_at_button.setEnabled(False)
enable_all_buttons_in_group(self.eeprom_group, True)
@QtCore.pyqtSlot()
def on_at_exited(self):
enable_all_buttons_in_group(self.at_group, False)
enable_all_buttons_in_group(self.eeprom_group, False)
self.enter_at_button.setEnabled(True)
self.at_mode_progressbar_disconnected()
@QtCore.pyqtSlot(object)
def on_enter_at_result(self, success):
if success:
self.logger.info("Entered AT mode successfully.")
self.at_entered.emit()
self.start_read_all_params_thread()
else:
self.logger.error("Failed to enter AT mode.")
self.at_mode_progressbar_disconnected()
self.enter_at_button.setEnabled(True)
show_error_box(
"Failed to enter AT mode. Make sure you are connected "
"to the correct device and selected the appropriate baud rate.")
@QtCore.pyqtSlot()
def on_exit_at_button_clicked(self):
self.configurator.exit_at_mode()
self.logger.info("Exited AT mode.")
self.at_exited.emit()
@QtCore.pyqtSlot()
def on_reboot_button_clicked(self):
self.configurator.reboot()
self.logger.info("Rebooted.")
enable_all_buttons_in_group(self.at_group, False)
enable_all_buttons_in_group(self.eeprom_group, False)
self.enter_at_button.setEnabled(True)
self.at_mode_progressbar_disconnected()
@QtCore.pyqtSlot()
def on_browse_button_clicked(self):
path = QtWidgets.QFileDialog.getOpenFileName(self, "Select Firmware", filter="Intel Hex file (*.ihx)")[0]
self.logger.debug("Firmware selected: %s", path)
if os.path.isfile(path):
self.file_lineedit.setText(path)
@QtCore.pyqtSlot()
def on_upload_button_clicked(self):
path = self.file_lineedit.text()
if os.path.isfile(path):
self.start_upload_firmware_thread(path)
else:
show_error_box("The specified firmware file '{}' does not exist.".format(path))
@QtCore.pyqtSlot()
def on_load_settings_button_clicked(self):
self.logger.debug("Load settings button clicked.")
self.start_read_all_params_thread()
def start_read_all_params_thread(self):
if self._read_params_thread.isRunning():
self.logger.warn("Trying to read EEPROM, but the responsible "
"thread is already running.")
show_error_box("Already reading EEPROM...")
else:
self.logger.info("Refreshing current EEPROM parameters.")
params = self.configurator.EEPROM_PARAMETERS.keys()
self.load_settings_progressbar.setMinimum(0)
self.load_settings_progressbar.setMaximum(0)
self._read_params_worker.set_params(params)
self._read_params_thread.start()
def start_write_params_thread(self):
if self._write_params_thread.isRunning():
show_error_box("Already copying parameters to radio.")
else:
params = self.get_current_params()
names = list(params.keys())
values = [params[name] for name in names]
self._write_params_worker.set_params(names, values)
self._write_params_thread.start()
def start_upload_firmware_thread(self, path):
if self._upload_firmware_thread.isRunning():
show_error_box("Already uploading firmware...")
else:
self.upload_button.setEnabled(False)
self.at_exited.emit()
self._upload_firmware_worker.set_path(path)
self._upload_firmware_thread.start()
def update_load_progressbar(self, progress, maximum):
self.load_settings_progressbar.setMaximum(maximum)
self.load_settings_progressbar.setValue(progress)
def update_copy_settings_progressbar(self, progress, maximum):
self.copy_settings_progressbar.setMaximum(maximum)
self.copy_settings_progressbar.setValue(progress)
@QtCore.pyqtSlot(object)
def on_read_params_result(self, params):
if params is None:
self.logger.error("Failed to read EEPROM parameters!")
show_error_box("Failed to read EEPROM!")
else:
self.logger.debug("Received params: %s", params)
writable_params = dict()
for name in params:
if radio_tools.Configurator.EEPROM_PARAMETERS[name]["readonly"]:
continue
writable_params[name] = params[name]
self.set_param_combobox(name, params[name])
self.radio_params = writable_params
@QtCore.pyqtSlot(object)
def on_write_params_result(self, result):
if not all(result):
show_error_box("Some parameters could not be written.")
@QtCore.pyqtSlot()
def on_set_default_button_clicked(self):
self.logger.debug("Set default button clicked.")
col = self.get_eeprom_default_column()
self.copy_column_to_desired(col)
def at_mode_progressbar_connecting(self):
self.at_mode_progressbar.setMaximum(0)
self.at_mode_progressbar.setMinimum(0)
self.at_mode_progressbar.setEnabled(True)
def at_mode_progressbar_connected(self):
self.at_mode_progressbar.setMaximum(1)
self.at_mode_progressbar.setMinimum(0)
self.at_mode_progressbar.setValue(1)
self.at_mode_progressbar.setEnabled(True)
def at_mode_progressbar_disconnected(self):
self.at_mode_progressbar.setMaximum(1)
self.at_mode_progressbar.setMinimum(0)
self.at_mode_progressbar.setValue(1)
self.at_mode_progressbar.setEnabled(False)
@QtCore.pyqtSlot()
def on_get_standard_settings_button_clicked(self):
self.set_standard_settings()
@QtCore.pyqtSlot()
def on_compare_button_clicked(self):
self.show_compare_dialog()
@QtCore.pyqtSlot()
def on_copy_settings_button_clicked(self):
if self.show_confirm_changes_dialog():
if self.show_eeprom_warning():
self.start_write_params_thread()
def set_standard_settings(self):
params = radio_tools.Configurator.get_default_params()
for name in params:
self.set_default_combobox(name)
def get_combobox_ref(self, name):
try:
combobox = getattr(self, name)
except AttributeError:
self.logger.warn("No widget with name: '%s' was found.", name)
return None
return combobox
def set_param_combobox(self, param_name, value):
name = param_name + "_combobox"
combobox = self.get_combobox_ref(name)
if combobox is None:
return False
index = combobox.findText(str(value))
if index < 0:
return False
else:
combobox.setCurrentIndex(index)
return True
def set_default_combobox(self, param_name):
try:
value = radio_tools.Configurator.get_default_params()[param_name]
except KeyError:
self.logger.error("Could not find default value for '%s'.",
param_name)
return False
return self.set_param_combobox(param_name, value)
def get_param_combobox(self, param_name):
name = param_name + "_combobox"
combobox = self.get_combobox_ref(name)
if combobox is None:
return None
return combobox.currentText()
def init_param_combobox(self, param_name, options, default):
options = [str(option) for option in options]
name = param_name + "_combobox"
combobox = self.get_combobox_ref(name)
if combobox is None:
return False
combobox.clear()
combobox.addItems(options)
if default is None:
return True
index = combobox.findText(str(default))
if index < 0:
return False
else:
combobox.setCurrentIndex(index)
return True
def init_param_widgets(self, set_default=False):
params = radio_tools.Configurator.EEPROM_PARAMETERS
for name in params:
if params[name]["readonly"]:
continue
options = params[name]["options"]
default = params[name]["default"] if set_default else None
self.init_param_combobox(name, options, default)
def get_current_params(self):
names = list(radio_tools.Configurator.get_default_params().keys())
params = dict()
for name in names:
params[name] = self.get_param_combobox(name)
return params
def get_changed_params(self):
current_params = self.get_current_params()
diff = dict()
for name in current_params:
old = int(self.radio_params[name])
new = int(current_params[name])
if old != new:
diff[name] = dict(old=old, new=new)
return diff
def show_compare_dialog(self):
diff = self.get_changed_params()
if diff:
text = "Following parameters changed:\n\n"
for name in diff:
text += "{}: {} -> {}\n".format(name, diff[name]["old"],
diff[name]["new"])
else:
text = "No paramters changed."
show_message_box(text)
def show_confirm_changes_dialog(self):
diff = self.get_changed_params()
if diff:
text = "Following parameters will be changed:\n\n"
for name in diff:
text += "{}: {} -> {}\n".format(name, diff[name]["old"],
diff[name]["new"])
else:
text = "No paramters changed."
text += "\n\n Continue?"
diag = QtWidgets.QMessageBox
reply = diag.question(self, "Confirm changes", text, diag.Yes | diag.No)
if reply == diag.Yes:
return True
else:
return False
def show_eeprom_warning(self):
text = (
"Parameters are going to be written to the EEPROM.\n\nKeep in "
"mind that the number of EEPROM writes is limited and this action "
"should be only performed if necessary.\n\nContinue?")
diag = QtWidgets.QMessageBox
reply = diag.warning(self, "Confirm EEPROM write", text,
diag.Yes | diag.No)
if reply == diag.Yes:
return True
else:
return False
def closeEvent(self, event):
self.do_disconnect()
event.accept()
class StepperWidget(Plugin):
x_position = 0
drill_position = 0
probe_position = 0
ext_position = 0
rot_position = 0
cur_x_position = 0
cur_drill_position = 0
cur_probe_position = 0
cur_ext_position = 0
cur_rot_position = 0
x_position_prev = 0
drill_position_prev = 0
probe_position_prev = 0
ext_position_prev = 0
rot_position_prev = 0
x_position_target = 0
drill_position_target = 0
probe_position_target = 0
ext_position_target = 0
rot_position_target = 0
dam_data_sig = QtCore.pyqtSignal(float, float)
pas_data_sig = QtCore.pyqtSignal(float, float, float)
def __init__(self, context):
super(StepperWidget, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('StepperWidget')
# 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()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('stepper_widget'),
'resource', 'stepper_widget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('StepperWidgetPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
####subscriers
self.dam_sub = rospy.Subscriber("dam_data", dam_data, self.dam_data_cb)
self.dam_data_sig.connect(self.dam_data_sig_cb)
self.pas_sub = rospy.Subscriber("pas_data", pas_data, self.pas_data_cb)
self.pas_data_sig.connect(self.pas_data_sig_cb)
###move buttons
self.x_move_pub = rospy.Publisher("x_axis_target",
UInt16,
queue_size=10)
self._widget.xMoveButton.clicked[bool].connect(self.x_move_cb)
self.drill_move_pub = rospy.Publisher("drill_y_axis_target",
UInt16,
queue_size=10)
self._widget.drillMoveButton.clicked[bool].connect(self.drill_move_cb)
self.probe_move_pub = rospy.Publisher("probe_y_axis_target",
UInt16,
queue_size=10)
self._widget.probeMoveButton.clicked[bool].connect(self.probe_move_cb)
self.rot_move_pub = rospy.Publisher("probe_rot_target",
UInt16,
queue_size=10)
self._widget.rotMoveButton.clicked[bool].connect(self.rot_move_cb)
self.ext_move_pub = rospy.Publisher("probe_ext_target",
UInt16,
queue_size=10)
self._widget.extMoveButton.clicked[bool].connect(self.ext_move_cb)
###sliders and boxes
self._widget.xSlider.valueChanged[int].connect(self.x_slider_cb)
self._widget.xBox.valueChanged[int].connect(self.x_box_cb)
self._widget.drillSlider.valueChanged[int].connect(
self.drill_slider_cb)
self._widget.drillBox.valueChanged[int].connect(self.drill_box_cb)
self._widget.probeSlider.valueChanged[int].connect(
self.probe_slider_cb)
self._widget.probeBox.valueChanged[int].connect(self.probe_box_cb)
self._widget.rotSlider.valueChanged.connect(self.rot_slider_cb)
self._widget.rotBox.valueChanged.connect(self.rot_box_cb)
self._widget.extSlider.valueChanged.connect(self.ext_slider_cb)
self._widget.extBox.valueChanged.connect(self.ext_box_cb)
### sliders and boxes ###
def x_box_cb(self, pos):
self.x_position = pos
self._widget.xSlider.setValue(pos)
def x_slider_cb(self, pos):
self.x_position = pos
self._widget.xBox.setValue(pos)
def drill_box_cb(self, pos):
self.drill_position = pos
self._widget.drillSlider.setValue(pos)
def drill_slider_cb(self, pos):
self.drill_position = pos
self._widget.drillBox.setValue(pos)
def probe_box_cb(self, pos):
self.probe_position = pos
self._widget.probeSlider.setValue(pos)
def probe_slider_cb(self, pos):
self.probe_position = pos
self._widget.probeBox.setValue(pos)
def rot_box_cb(self, pos):
self.rot_position = pos
self._widget.rotSlider.setValue(pos)
def rot_slider_cb(self, pos):
self.rot_position = pos
self._widget.rotBox.setValue(pos)
def ext_box_cb(self, pos):
self.ext_position = pos
self._widget.extSlider.setValue(pos)
def ext_slider_cb(self, pos):
self.ext_position = pos
self._widget.extBox.setValue(pos)
### move buttons ###
def x_move_cb(self, var):
self.x_move_pub.publish(self.x_position)
self.x_position_prev = self.cur_x_position
self.x_position_target = self.x_position
def drill_move_cb(self, var):
self.drill_move_pub.publish(self.drill_position)
self.drill_position_prev = self.cur_drill_position
self.drill_position_target = self.drill_position
def probe_move_cb(self, var):
self.probe_move_pub.publish(self.probe_position)
self.probe_position_prev = self.cur_probe_position
self.probe_position_target = self.probe_position
def ext_move_cb(self, var):
self.ext_move_pub.publish(self.ext_position)
self.ext_position_prev = self.cur_ext_position
self.ext_position_target = self.ext_position
def rot_move_cb(self, var):
self.rot_move_pub.publish(self.rot_position)
self.rot_position_prev = self.cur_rot_position
self.rot_position_target = self.rot_position
### dam and pas data ###
def dam_data_cb(self, msg):
self.dam_data_sig.emit(msg.stp_x1, msg.stp_y)
def dam_data_sig_cb(self, stp_x, stp_drill):
self._widget.xPosLabel.setText(str(round(stp_x, 2)))
self._widget.drillPosLabel.setText(str(round(stp_drill, 2)))
self.cur_x_position = stp_x
self.cur_drill_position = stp_drill
self._widget.xBar.setValue(
0 if self.x_position_target - self.x_position_prev == 0 else 100 *
abs(stp_x - self.x_position_prev) /
float(abs(self.x_position_target - self.x_position_prev)))
self._widget.drillBar.setValue(
0 if self.drill_position_target -
self.drill_position_prev == 0 else 100 *
abs(stp_drill - self.drill_position_prev) /
abs(self.drill_position_target - self.drill_position_prev))
def pas_data_cb(self, msg):
self.pas_data_sig.emit(msg.stp_y, msg.stp_rot, msg.stp_ext)
def pas_data_sig_cb(self, stp_probe, stp_rot, stp_ext):
self._widget.probePosLabel.setText(str(round(stp_probe, 2)))
self._widget.rotPosLabel.setText(str(round(stp_rot, 2)))
self._widget.extPosLabel.setText(str(round(stp_ext, 2)))
self.cur_probe_position = stp_probe
self.cur_rot_position = stp_rot
self.cur_ext_position = stp_ext
self._widget.probeBar.setValue(
0 if self.probe_position_target -
self.probe_position_prev == 0 else 100 *
abs(stp_probe - self.probe_position_prev) /
abs(self.probe_position_target - self.probe_position_prev))
self._widget.rotBar.setValue(0 if self.rot_position_target -
self.rot_position_prev == 0 else 100 *
abs(stp_rot - self.rot_position_prev) /
abs(self.rot_position_target -
self.rot_position_prev))
self._widget.extBar.setValue(0 if self.ext_position_target -
self.ext_position_prev == 0 else 100 *
abs(stp_ext - self.ext_position_prev) /
abs(self.ext_position_target -
self.ext_position_prev))
##UI class overrides
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class QtPart(QtGui.QWidget):
accepted = QtCore.pyqtSignal(list)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
loadUi(os.path.join(path, 'resources', 'part.ui'), self)
self.pub_marker_array = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=1)
self.btnLoad.clicked.connect(self.btnLoadClicked)
self.btnProcessMesh.clicked.connect(self.btnProcessMeshClicked)
self.btnProcessLayer.clicked.connect(self.btnProcessLayerClicked)
self.btnAcceptPath.clicked.connect(self.btnAcceptPathClicked)
self.sbPositionX.valueChanged.connect(self.changePosition)
self.sbPositionY.valueChanged.connect(self.changePosition)
self.sbPositionZ.valueChanged.connect(self.changePosition)
self.sbSizeX.valueChanged.connect(self.changeSize)
self.sbSizeY.valueChanged.connect(self.changeSize)
self.sbSizeZ.valueChanged.connect(self.changeSize)
self.processing = False
self.timer = QtCore.QTimer(self)
self.timer.timeout.connect(self.updateProcess)
self.robpath = RobPath()
def btnLoadClicked(self):
self.blockSignals(True)
filename = QtGui.QFileDialog.getOpenFileName(
self, 'Open file', os.path.join(path, 'data'),
'Mesh Files (*.stl)')[0]
print 'Filename:', filename
self.setWindowTitle(filename)
self.robpath.load_mesh(filename)
self.mesh_size = self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1
self.updatePosition(self.robpath.mesh.bpoint1) # Rename to position
self.updateSize(self.robpath.mesh.bpoint2 - self.robpath.mesh.bpoint1)
self.marker_array = MarkerArray()
#self.marker = MeshMarker(mesh_resource="file://"+filename, frame_id="/workobject")
self.mesh = TriangleListMarker()
self.mesh.set_frame('/workobject')
self.mesh.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.mesh.set_color((0.66, 0.66, 0.99, 0.66))
self.marker_array.markers.append(self.mesh.marker)
self.path = LinesMarker()
self.path.set_frame('/workobject')
self.path.set_color((1.0, 0.0, 0.0, 1.0))
self.marker_array.markers.append(self.path.marker)
for id, m in enumerate(self.marker_array.markers):
m.id = id
self.npoints = 0
# #rospy.loginfo()
self.pub_marker_array.publish(self.marker_array)
#TODO: Change bpoints.
#self.lblInfo.setText('Info:\n')
#self.plot.drawMesh(self.robpath.mesh)
#TODO: Add info from velocity estimation module.
self.blockSignals(False)
def updateParameters(self):
height = self.sbHeight.value()
width = self.sbWidth.value()
overlap = 0.01 * self.sbOverlap.value()
print height, width, overlap
self.robpath.set_track(height, width, overlap)
def updateProcess(self):
if self.robpath.k < len(self.robpath.levels):
self.robpath.update_process()
#self.plot.drawSlice(self.robpath.slices, self.robpath.path)
points = np.array(
[pose[0] for pose in self.robpath.path[self.npoints:-1]])
self.npoints = self.npoints + len(points)
self.path.set_points(0.001 * points)
print len(points)
self.pub_marker_array.publish(self.marker_array)
#self.plot.progress.setValue(100.0 * self.robpath.k / len(self.robpath.levels))
else:
self.processing = False
self.timer.stop()
def btnProcessMeshClicked(self):
if self.processing:
self.processing = False
self.timer.stop()
else:
self.updateParameters()
self.robpath.init_process()
self.processing = True
self.timer.start(100)
def btnProcessLayerClicked(self):
if self.processing:
self.updateParameters()
self.updateProcess()
else:
self.updateParameters()
self.robpath.init_process()
self.processing = True
def btnAcceptPathClicked(self):
self.accepted.emit(self.robpath.path)
def updatePosition(self, position):
x, y, z = position
self.sbPositionX.setValue(x)
self.sbPositionY.setValue(y)
self.sbPositionZ.setValue(z)
def changePosition(self):
x = self.sbPositionX.value()
y = self.sbPositionY.value()
z = self.sbPositionZ.value()
self.robpath.translate_mesh(np.float32([x, y, z]))
#self.marker.set_position((x, y, z))
self.mesh.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.pub_marker_array.publish(self.marker_array)
def updateSize(self, size):
sx, sy, sz = size
self.sbSizeX.setValue(sx)
self.sbSizeY.setValue(sy)
self.sbSizeZ.setValue(sz)
def changeSize(self):
sx = self.sbSizeX.value()
sy = self.sbSizeY.value()
sz = self.sbSizeZ.value()
self.robpath.resize_mesh(np.float32([sx, sy, sz]))
#scale = np.float32([sx, sy, sz]) / self.mesh_size
self.mesh.set_points(0.001 * np.vstack(self.robpath.mesh.triangles))
self.pub_marker_array.publish(self.marker_array)
def blockSignals(self, value):
self.sbPositionX.blockSignals(value)
self.sbPositionY.blockSignals(value)
self.sbPositionZ.blockSignals(value)
self.sbSizeX.blockSignals(value)
self.sbSizeY.blockSignals(value)
self.sbSizeZ.blockSignals(value)
class LoggingWidget(QtGui.QWidget):
__logging_cb_signal = QtCore.pyqtSignal(String)
__start_logging_signal = QtCore.pyqtSignal()
__stop_logging_signal = QtCore.pyqtSignal()
def __init__(self):
QtGui.QWidget.__init__(self)
self._status = "unknown"
self._layout = QtGui.QGridLayout(self)
self._widgets = dict()
self.__logging_cb_signal.connect(self.logging_cb)
self.__start_logging_signal.connect(self.start_logging)
self.__stop_logging_signal.connect(self.stop_logging)
self._add_widget("pid_label", QtGui.QLabel("PID:", self))
self._add_widget("pid", QtGui.QLineEdit(self))
self._widgets["pid"].setMaximumWidth(50)
self._add_widget("label_status", QtGui.QLabel(self))
self._widgets["label_status"].setText("Status: Unknown")
self._widgets["label_status"].setTextFormat(QtCore.Qt.RichText)
self._add_widget("button_start", QtGui.QPushButton("START", self))
self._add_widget("button_stop", QtGui.QPushButton("STOP", self))
self._widgets["button_start"].clicked.connect(
self.__start_logging_signal.emit)
self._widgets["button_stop"].clicked.connect(
self.__stop_logging_signal.emit)
self._widgets["label_warning"] = QtGui.QLabel(self)
self._layout.addWidget(self._widgets["label_warning"], 1, 0, 1, 2)
self._widgets["label_warning"].setTextFormat(QtCore.Qt.RichText)
rospy.Subscriber("/data_logger/status", String,
self.__logging_cb_signal.emit)
self._logging_start = rospy.ServiceProxy('/data_logger/start',
DataLoggerStart)
self._logging_stop = rospy.ServiceProxy('/data_logger/stop',
DataLoggerStop)
def _add_widget(self, name, widget):
""" adds widget to dictionary and puts it in the layout, in order """
self._widgets[name] = widget
self._layout.addWidget(self._widgets[name], 0,
len(self._widgets.keys()) - 1)
def logging_cb(self, data):
self._status = data.data
if data.data == "stopped":
self._widgets["button_stop"].setEnabled(False)
self._widgets["button_start"].setEnabled(True)
self._widgets["label_status"].setText("Status: %s" % data.data)
elif data.data == "started":
self._widgets["button_stop"].setEnabled(True)
self._widgets["button_start"].setEnabled(False)
self._widgets["label_status"].setText(
"Status: <span style='color:#00aa00;'>%s</span>" % data.data)
def start_logging(self):
pid = self._widgets["pid"].text()
log_path = LOG_DEST + "%s" % pid
log_path = os.path.abspath(os.path.expanduser(log_path))
# Make the directory
if not os.path.exists(log_path):
print "Creating destination directory '%s'" % log_path
os.makedirs(log_path)
try:
self._logging_start(log_path, "--all")
except Exception as e:
print "Failed to start logging service:"
print str(e)
def stop_logging(self):
try:
self._logging_stop()
except Exception as e:
print "Failed to stop logging service:"
print str(e)
class MonitorPlugin(Plugin):
sensor0_id = 0
sensor0_sum = 0
sensor1_id = 0
sensor1_sum = 0
do_update_ui = QtCore.pyqtSignal(int, int, int)
def __init__(self, context):
super(MonitorPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('MonitorPlugin')
# 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()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_monitor_robochoto'), 'resource', 'MonitorPlugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('MonitorPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
print("subscribiendose a los topicos")
self._sensor0_sub = rospy.Subscriber("/robot/sensor_0_value", UInt16, self._sensor0_cb)
self._sensor1_sub = rospy.Subscriber("/robot/sensor_1_value", UInt16, self._sensor1_cb)
self._detection_sub = rospy.Subscriber("/robot/mine_detected", UInt8, self._detection_cb)
self.do_update_ui.connect(self._update_ui)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
#def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def _update_ui(self, s0val, s1val, detect):
if detect == 0:
self._widget.sensor0_event.setStyleSheet('background: green')
self._widget.sensor1_event.setStyleSheet('background: green')
elif detect == 1:
self._widget.sensor0_event.setStyleSheet('background: red')
self._widget.sensor1_event.setStyleSheet('background: green')
elif detect == 2:
self._widget.sensor0_event.setStyleSheet('background: green')
self._widget.sensor1_event.setStyleSheet('background: red')
elif detect == 3:
self._widget.sensor0_event.setStyleSheet('background: red')
self._widget.sensor1_event.setStyleSheet('background: red')
if s0val >=0:
self._widget.front_sensor_level.setValue(s0val)
self._widget.superficial_counter.display(s0val)
if s1val >=0:
self._widget.rear_sensor_level.setValue(s1val)
self._widget.burried_counter.display(s1val)
def _button_cb(self):
print("hola")
#idbutton = self._widget.sender()
def _detection_cb(self, msg):
self.do_update_ui.emit(-1, -1, msg.data)
def _sensor0_cb(self, msg):
self.sensor0_sum += msg.data / 10
self.sensor0_id += 1
if self.sensor0_id > 5:
avg = self.sensor0_sum / self.sensor0_id
self.do_update_ui.emit(avg, -1, 5)
#self._widget.front_sensor_level.setText("")
self.sensor0_id = 0
self.sensor0_sum = 0
def _sensor1_cb(self, msg):
self.sensor1_sum += msg.data / 10
self.sensor1_id += 1
if self.sensor1_id > 5:
avg = self.sensor1_sum / self.sensor1_id
self.do_update_ui.emit(-1, avg, 5)
self.sensor1_id = 0
self.sensor1_sum = 0
class StateWidget(Plugin):
pas_state = {
-1: "E_STOP",
0: "DEFAULT_STATE",
1: "HOMED",
2: "HOMING_PROBE",
3: "HOMING",
4: "BOWL",
5: "ROCKWELL"
}
dam_state = {
-1: "E_STOP",
0: "DEFAULT_STATE",
1: "HOMED",
2: "HOMING",
3: "HOMING_DRILL",
4: "DRILLING"
}
dam_data_sig = QtCore.pyqtSignal(int)
pas_data_sig = QtCore.pyqtSignal(int)
def __init__(self, context):
super(StateWidget, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('StateWidget')
# 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()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('state_widget'),
'resource', 'state_widget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('StateWidgetPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
####subscriers
self.dam_sub = rospy.Subscriber("dam_data", dam_data, self.dam_data_cb)
self.dam_data_sig.connect(self.dam_data_sig_cb)
self.pas_sub = rospy.Subscriber("pas_data", pas_data, self.pas_data_cb)
self.pas_data_sig.connect(self.pas_data_sig_cb)
####buttons
self.estop_pub = rospy.Publisher("estop", Empty, queue_size=10)
self._widget.estopButton.clicked[bool].connect(
lambda: self.estop_pub.publish(Empty()))
self.reset_pub = rospy.Publisher("reset", Empty, queue_size=10)
self._widget.resetButton.clicked[bool].connect(
lambda: self.restart_pub.publish(Empty()))
self.resume_pub = rospy.Publisher("resume", Empty, queue_size=10)
self._widget.resumeButton.clicked[bool].connect(
lambda: self.resume_pub.publish(Empty()))
### dam and pas data ###
def dam_data_cb(self, msg):
self.dam_data_sig.emit(msg.state)
def dam_data_sig_cb(self, state):
self._widget.damLabel.setText(self.dam_state[state])
def pas_data_cb(self, msg):
self.pas_data_sig.emit(msg.state)
def pas_data_sig_cb(self, state):
self._widget.probePosLabel.setValue(self.pas_state[state])
##UI class overrides
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class SpectrogramPlugin(Plugin):
update_signal = QtCore.pyqtSignal()
subscriber_signal = QtCore.pyqtSignal(str)
def __init__(self, context):
super(SpectrogramPlugin, self).__init__(context)
self.setObjectName('Spectrogram')
self._widget = QWidget()
layout = QVBoxLayout()
self._widget.setLayout(layout)
layout_ = QHBoxLayout()
self.line_edit = QLineEdit()
layout_.addWidget(self.line_edit)
self.apply_btn = QPushButton("Apply")
self.apply_btn.clicked.connect(self.apply_clicked)
layout_.addWidget(self.apply_btn)
layout.addLayout(layout_)
self.fig = Figure((5, 4), dpi=100)
self.canvas = FigureCanvas(self.fig)
self.axes = self.fig.add_subplot(111)
self.fig.tight_layout()
layout.addWidget(self.canvas)
context.add_widget(self._widget)
self.update_signal.connect(self.update_spectrogram)
self.subscriber_signal.connect(self.update_subscriber)
self.subscriber_signal.emit('spec')
def spectrum_callback(self, data):
nch = data.nch
len = data.nfreq
if self.spectrogram is None:
self.spectrogram = zeros([len, 1000])
s = array(data.data).reshape([nch, len, 2])[-1]
s = linalg.norm(s, axis=1)
s += 1e-8
log(s, s)
self.spectrogram = roll(self.spectrogram, -1, 1)
self.spectrogram[:, -1] = s
if data.header.seq % 100 == 0:
self.update_signal.emit()
def apply_clicked(self):
self.update_subscriber(self.line_edit.displayText())
def update_spectrogram(self):
if self.spectrogram is not None:
self.axes.clear()
self.axes.imshow(self.spectrogram,
aspect="auto",
origin="lower",
cmap="hot")
self.axes.grid(None)
self.axes.set_ylabel("Freq. [bin]")
self.axes.set_xlabel("Time [frame]")
self.fig.tight_layout()
self.canvas.draw()
QApplication.processEvents()
def update_subscriber(self, topic_name):
self.topic_name = topic_name
self.line_edit.setText(self.topic_name)
if hasattr(self, 'sub'):
self.sub.unregister()
self.spectrogram = None
self.sub = rospy.Subscriber(topic_name,
Spectrum,
self.spectrum_callback,
queue_size=500)
def restore_settings(self, plugin_settings, instance_settings):
topic_name = instance_settings.value('topic_name')
self.subscriber_signal.emit(topic_name)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('topic_name', self.topic_name)
def shutdown_plugin(self):
pass
class InspectionWindow(QtWidgets.QDialog):
"""This window operates as the main interaction with the robot under inspection missions"""
activate = QtCore.pyqtSignal(bool)
def __init__(self):
super(InspectionWindow, self).__init__()
self.setAttribute(QtCore.Qt.WA_DeleteOnClose)
self.ui = '../forms/manual.ui'
self.mission_dir = '../instance/missions'
loadUi(self.ui, self)
self.setWindowFlags(QtCore.Qt.FramelessWindowHint)
self.layout = self.findChild(QtWidgets.QGridLayout, 'layout')
self.table_row_tracker = 2
self.num_goal_reached = 0
# Image frames
self.videoframe = self.findChild(QtWidgets.QLabel, 'videoFrame')
self.topImageLabel = self.findChild(QtWidgets.QLabel, 'topImageLabel')
self.middelImageLabel = self.findChild(QtWidgets.QLabel, 'middelImageLabel')
self.bottomImageLabel = self.findChild(QtWidgets.QLabel, 'bottomImageLabel')
# Buttons
self.startVideoStreamBtn = self.findChild(QtWidgets.QPushButton, 'startVideoStreamBtn')
self.stopVideoStreamBtn = self.findChild(QtWidgets.QPushButton, 'stopVideoStreamBtn')
self.abortMissionBtn = self.findChild(QtWidgets.QToolButton, 'abortMissionBtn')
self.inspectStatusBtn = self.findChild(QtWidgets.QPushButton, 'inspectStatusBtn')
self.pauseMissionBtn = self.findChild(QtWidgets.QToolButton, 'pauseMissionBtn')
self.stopMissionBtn = self.findChild(QtWidgets.QToolButton, 'stopMissionBtn')
self.startMissionBtn = self.findChild(QtWidgets.QToolButton, 'startMissionBtn')
self.refreshSelectMissionAreaBtn = self.findChild(QtWidgets.QToolButton, 'refreshSelectMissionAreaBtn')
self.refreshSelectMissionBtn = self.findChild(QtWidgets.QToolButton, 'refreshSelectMissionBtn')
self.inspectStatusBtn.hide()
# Button connections
self.startMissionBtn.clicked.connect(self.start_mission)
self.pauseMissionBtn.clicked.connect(self.pause_mission)
# Labels
self.runninMissionLabel = self.findChild(QtWidgets.QLabel, 'runninMissionLabel')
self.runningTaskLabel = self.findChild(QtWidgets.QLabel, 'runningTaskLabel')
self.currentRunningMissionLabel = self.findChild(QtWidgets.QLabel, 'currentRunningMissionLabel')
# Progressbars
self.runningTaskProgressBar = self.findChild(QtWidgets.QProgressBar, 'runningTaskProgressBar')
# Comboboxes
self.videoSourceComboBox = self.findChild(QtWidgets.QComboBox, 'videoSourceComboBox')
self.selectMissionAreaComboBox = self.findChild(QtWidgets.QComboBox, 'selectMissionAreaComboBox')
self.selectMissionComboBox = self.findChild(QtWidgets.QComboBox, 'selectMissionComboBox')
self.videoSourceComboBox.addItems(["Color", "Fisheye 1", "Fisheye 2","Infared 1", "Infared 2", ])
self.selectMissionAreaComboBox.addItems(['', "Workshop", "University", "Demo 1","Demo 2", "Demo 3"])
self.find_missions()
# RVIZ
self.visual_frame = rviz.VisualizationFrame()
self.visual_frame.setSplashPath("")
self.visual_frame.initialize()
self.add_rviz_config()
self.visual_frame.setMenuBar(None)
self.visual_frame.setStatusBar(None)
self.visual_frame.setHideButtonVisibility(True)
self.layout.addWidget(self.visual_frame)
self.manager = self.visual_frame.getManager()
self.grid_display = self.manager.getRootDisplayGroup().getDisplayAt(0)
self.plan = None
self.missions = []
self.column_images = [self.topImageLabel, self.middelImageLabel, self.bottomImageLabel]
self.column_image_counter = 0
# Tables
self.tableWidget = self.findChild(QtWidgets.QTableWidget, 'tableWidget')
self.tableWidget.setHorizontalHeaderLabels(['Time', 'Tag', 'Operation','Status', 'Value', 'Warning', 'Error'])
header = self.tableWidget.horizontalHeader()
header.setSectionResizeMode(0, QtWidgets.QHeaderView.Stretch)
header.setSectionResizeMode(1, QtWidgets.QHeaderView.Stretch)
header.setSectionResizeMode(2, QtWidgets.QHeaderView.Stretch)
header.setSectionResizeMode(3, QtWidgets.QHeaderView.Stretch)
header.setSectionResizeMode(4, QtWidgets.QHeaderView.Stretch)
header.setSectionResizeMode(5, QtWidgets.QHeaderView.Stretch)
header.setSectionResizeMode(6, QtWidgets.QHeaderView.Stretch)
# Top Buttons
self.change_mode_btn = self.findChild(
QtWidgets.QPushButton, 'changeModeBtn')
self.exit_btn = self.findChild(QtWidgets.QPushButton, 'exitBtn')
self.powerBtn = self.findChild(QtWidgets.QPushButton, 'powerBtn')
self.emergency_btn = self.findChild(
QtWidgets.QPushButton, 'emergencyBtn')
# Status indicators
self.signal_btn = self.findChild(QtWidgets.QPushButton, 'signalBtn')
self.controller_battery_btn = self.findChild(
QtWidgets.QPushButton, 'controllerBatteryBtn')
self.battery_btn = self.findChild(QtWidgets.QPushButton, 'batteryBtn')
self.health_btn = self.findChild(QtWidgets.QPushButton, 'healthBtn')
# Button connections
# Button shortcuts
self.exit_btn.setShortcut("Ctrl+Q")
# Stylesheets
self.powerBtn.setStyleSheet(
"QPushButton#powerBtn:checked {color:black; background-color: red;}")
self.signal_btn.setStyleSheet(
"QPushButton#signalBtn:checked {color:black; background-color: green;}")
# ROS SUBSCRIBERS
rospy.init_node('listener', anonymous=True)
rospy.Subscriber('/d435/infra1/image_rect_raw', Image, self.image_callback)
rospy.Subscriber('goal_reached', UInt8, self.result_callback)
rospy.Subscriber('in_position', String, self.api_callback)
self.show()
def add_rviz_config(self):
"""Add rviz configurations from external file"""
reader = rviz.YamlConfigReader()
config = rviz.Config()
reader.readFile(config, "../instance/Sparkie.rviz")
self.visual_frame.load(config)
def start_mission(self):
"""Start selected mission"""
choice = QtWidgets.QMessageBox.question(self, 'Warning', 'Start new mission?', QtWidgets.QMessageBox.Yes | QtWidgets.QMessageBox.No)
if choice == QtWidgets.QMessageBox.Yes:
self.load_mission()
self.startMissionBtn.setDisabled(True)
self.refreshSelectMissionAreaBtn.setDisabled(True)
self.refreshSelectMissionBtn.setDisabled(True)
self.selectMissionAreaComboBox.setDisabled(True)
self.selectMissionComboBox.setDisabled(True)
self.runninMissionLabel.setText(self.selectMissionComboBox.currentText())
self.currentRunningMissionLabel.setText(self.selectMissionComboBox.currentText())
self.tableWidget.setItem(0,0, QtWidgets.QTableWidgetItem(str(datetime.datetime.fromtimestamp(rospy.get_time()))))
self.tableWidget.setItem(0,1, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(0,2, QtWidgets.QTableWidgetItem('Starting Mission'))
self.tableWidget.setItem(0,3, QtWidgets.QTableWidgetItem('Init'))
self.tableWidget.setItem(0,4, QtWidgets.QTableWidgetItem('Success'))
self.tableWidget.setItem(0,5, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(0,6, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(0,7, QtWidgets.QTableWidgetItem('N/A'))
self.runningTaskLabel.setText('Starting Mission')
self.tableWidget.setItem(1,0, QtWidgets.QTableWidgetItem(str(datetime.datetime.fromtimestamp(rospy.get_time()))))
self.tableWidget.setItem(1,1, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(1,2, QtWidgets.QTableWidgetItem('Move to new waypoint'))
self.tableWidget.setItem(1,3, QtWidgets.QTableWidgetItem('Sent'))
self.tableWidget.setItem(1,4, QtWidgets.QTableWidgetItem('Ongoing'))
self.tableWidget.setItem(1,5, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(1,6, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(1,7, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(2,0, QtWidgets.QTableWidgetItem(''))
self.runningTaskLabel.setText('Move to new waypoint')
self.post_goal()
else:
pass
def result_callback(self, data):
"""Goal reached callback"""
if data.data:
self.runningTaskProgressBar.setValue(self.num_goal_reached)
self.num_goal_reached += 1
print(self.num_goal_reached)
print("Goal Reached, ready for new one")
self.tableWidget.setItem(self.table_row_tracker, 0, QtWidgets.QTableWidgetItem(str(datetime.datetime.fromtimestamp(rospy.get_time()))))
self.tableWidget.setItem(self.table_row_tracker, 1, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 2, QtWidgets.QTableWidgetItem('Reached waypoint'))
self.tableWidget.setItem(self.table_row_tracker, 3, QtWidgets.QTableWidgetItem('Success'))
self.tableWidget.setItem(self.table_row_tracker, 4, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 5, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 6, QtWidgets.QTableWidgetItem('N/A'))
self.runningTaskLabel.setText('Reached waypoint')
self.table_row_tracker += 1
def image_callback(self, data):
"""Image callback"""
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
rgb_image = CvBridge().imgmsg_to_cv2(data, desired_encoding="rgb8")
height, width, channel = rgb_image.shape
bytesPerLine = 3 * width
qImg = QtGui.QImage(rgb_image.data, width, height, bytesPerLine, QtGui.QImage.Format_RGB888)
self.videoframe.setPixmap(QtGui.QPixmap(qImg))
def api_callback(self, data):
"""API callback from cloud"""
print("Robot in position")
self.tableWidget.setItem(self.table_row_tracker, 0, QtWidgets.QTableWidgetItem(str(datetime.datetime.fromtimestamp(rospy.get_time()))))
self.tableWidget.setItem(self.table_row_tracker, 1, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 2, QtWidgets.QTableWidgetItem('Move to new waypoint'))
self.tableWidget.setItem(self.table_row_tracker, 3, QtWidgets.QTableWidgetItem('Sent'))
self.tableWidget.setItem(self.table_row_tracker, 4, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 5, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 6, QtWidgets.QTableWidgetItem('N/A'))
self.runningTaskLabel.setText('Move to new waypoint')
self.table_row_tracker += 1
command = 'python post_goal.py %s' % self.num_goal_reached
_id = rospy.get_caller_id()
_class = self.plan[self.num_goal_reached-1]
print(_class)
if len(_class) > 3:
self.tableWidget.setItem(self.table_row_tracker, 0, QtWidgets.QTableWidgetItem(str(datetime.datetime.fromtimestamp(rospy.get_time()))))
self.tableWidget.setItem(self.table_row_tracker, 1, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 2, QtWidgets.QTableWidgetItem('Move head to position'))
self.tableWidget.setItem(self.table_row_tracker, 3, QtWidgets.QTableWidgetItem('Ongoing'))
self.tableWidget.setItem(self.table_row_tracker, 4, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 5, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 6, QtWidgets.QTableWidgetItem('N/A'))
self.runningTaskLabel.setText('Move head to position')
self.table_row_tracker += 1
time.sleep(10)
self.tableWidget.setItem(self.table_row_tracker-1, 3, QtWidgets.QTableWidgetItem('Complete'))
self.post_goal()
print("New goal sent, waiting for database to update")
time.sleep(8)
URL = 'http://dr0nn1.ddns.net:5000/%s/1' % _class
response = requests.get(URL)
content = response.json()
tag = 'N/A'
value = 'N/A'
warning = 'N/A'
alarm = 'N/A'
value = 'N/A'
if _class == 'manometers':
value = float(content['value'])
warning_low = float(content['low_warning_limit'])
warning_high = float(content['high_warning_limit'])
alarm_low = float(content['low_alarm_limit'])
alarm_high = float(content['high_alarm_limit'])
if value > warning_high and value < alarm_high:
warning = 'Warning high'
elif value < warning_low and value > alarm_low:
warning = 'Warning low'
elif value > alarm_high:
alarm = 'Alarm high'
elif value < alarm_low:
alarm = 'Alarm low'
tag = content['tag']
if _class == 'valve':
normal_condition = content['normal_condition']
should_be = content['is_open']
tag = content['tag']
if value is not should_be:
warning = 'Not in position'
IMG = content['img'].encode('latin1')
rgb_image = np.fromstring(IMG, dtype=np.uint8).reshape((480, 640, 3))
height, width, channel = rgb_image.shape
bytesPerLine = 3 * width
qImg = QtGui.QImage(rgb_image.data, width, height, bytesPerLine, QtGui.QImage.Format_RGB888)
self.column_images[self.column_image_counter].setPixmap(QtGui.QPixmap(qImg))
self.tableWidget.setItem(self.table_row_tracker, 0, QtWidgets.QTableWidgetItem(str(datetime.datetime.fromtimestamp(rospy.get_time()))))
self.tableWidget.setItem(self.table_row_tracker, 1, QtWidgets.QTableWidgetItem(tag))
self.tableWidget.setItem(self.table_row_tracker, 2, QtWidgets.QTableWidgetItem('Inspecting equipment'))
self.tableWidget.setItem(self.table_row_tracker, 3, QtWidgets.QTableWidgetItem('Complete'))
self.tableWidget.setItem(self.table_row_tracker, 4, QtWidgets.QTableWidgetItem(value))
self.tableWidget.setItem(self.table_row_tracker, 5, QtWidgets.QTableWidgetItem(warning))
self.tableWidget.setItem(self.table_row_tracker, 6, QtWidgets.QTableWidgetItem(alarm))
self.runningTaskLabel.setText('Inspecting equipment')
self.table_row_tracker += 1
self.column_image_counter += 1
if self.column_image_counter > 2:
self.column_image_counter = 0
else:
time.sleep(2)
self.post_goal()
def post_goal(self):
"""Post new waypoint to subsystem"""
th = threading.Thread(target=self._post_goal)
th.start()
def _post_goal(self):
"""Post new waypoint to subsystem"""
print('Posting new goal to agent')
self.tableWidget.setItem(self.table_row_tracker, 0, QtWidgets.QTableWidgetItem(str(datetime.datetime.fromtimestamp(rospy.get_time()))))
self.tableWidget.setItem(self.table_row_tracker, 1, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 2, QtWidgets.QTableWidgetItem('Move to new waypoint'))
self.tableWidget.setItem(self.table_row_tracker, 3, QtWidgets.QTableWidgetItem('Sent'))
self.tableWidget.setItem(self.table_row_tracker, 4, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 5, QtWidgets.QTableWidgetItem('N/A'))
self.tableWidget.setItem(self.table_row_tracker, 6, QtWidgets.QTableWidgetItem('N/A'))
self.runningTaskLabel.setText('Move to new waypoint')
self.table_row_tracker += 1
command = 'python post_goal.py %s' % self.num_goal_reached
subprocess.call(command, shell=True)
def power_on(self):
"""Turn on interface"""
active = self.powerBtn.isChecked()
if active:
self.activate.emit(True)
else:
self.activate.emit(False)
def close_window(self):
"""Close window Ctrl+Q"""
self.close()
def turn_robot_off(self):
pass
def pause_mission(self):
pass
def load_mission(self):
"""Preload missions"""
mission = self.missions[self.selectMissionComboBox.currentIndex()]
with open(mission, 'r') as file:
self.plan = file.read().split(',')
def find_missions(self):
"""Find missionfiles in mission directory"""
for file in os.listdir(self.mission_dir):
if file.endswith(".txt"):
self.missions.append(os.path.join(self.mission_dir, file))
self.selectMissionComboBox.addItem(file)
class Dashboard(Plugin):
autoStateSignal = QtCore.pyqtSignal(int)
nBallsSignal = QtCore.pyqtSignal(int)
shooterInRangeSignal = QtCore.pyqtSignal(bool)
turretInRangeSignal = QtCore.pyqtSignal(bool)
msg_data = "default"
def __init__(self, context):
#Start client -rosbridge
self.client = roslibpy.Ros(host='localhost', port=5803)
self.client.run()
super(Dashboard, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Dashboard')
# 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()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_dashboard'), 'resource', 'Dashboard.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('DashboardUi')
# Set up signal-slot connections
self._widget.set_imu_angle_button.clicked.connect(self.setImuAngle)
self._widget.imu_angle.valueChanged.connect(self.imuAngleChanged)
self._widget.auto_wall_dist_button.clicked.connect(self.setAutoWallDist)
self._widget.auto_wall_dist.valueChanged.connect(self.autoWallDistChanged)
self._widget.ball_reset_button.clicked.connect(self.resetBallCount)
self._widget.ball_reset_count.valueChanged.connect(self.resetBallChanged)
# Add buttons for auto modes
v_layout = self._widget.auto_mode_v_layout #vertical layout storing the buttons
self.auto_mode_button_group = QButtonGroup(self._widget) # needs to be a member variable so the publisher can access it to see which auto mode was selected
# Search for auto_mode config items
for i in range(1,100): # loop will exit when can't find the next auto mode, so really only a while loop needed, but exiting at 100 will prevent infinite looping
if rospy.has_param("/auto/auto_mode_" + str(i)):
auto_sequence = rospy.get_param("/auto/auto_mode_" + str(i))
new_auto_mode = QWidget()
new_h_layout = QHBoxLayout()
new_h_layout.setContentsMargins(0,0,0,0)
new_button = QRadioButton("Mode " + str(i))
new_button.setStyleSheet("font-weight: bold")
self.auto_mode_button_group.addButton(new_button, i) #second arg is the button's id
new_h_layout.addWidget( new_button )
new_h_layout.addWidget( QLabel(", ".join(auto_sequence)) )
hSpacer = QSpacerItem(0, 0, QSizePolicy.Expanding, QSizePolicy.Minimum)
new_h_layout.addItem(hSpacer)
new_auto_mode.setLayout( new_h_layout )
v_layout.addWidget(new_auto_mode)
else:
print(str(i-1) + " auto modes found.")
# if no auto modes found, inform the user with a label
if (i-1) == 0:
v_layout.addWidget( QLabel("No auto modes found") )
break #break out of for loop searching for auto modes
# auto state stuff
self.autoState = 0
self.displayAutoState() #display initial auto state
# publish thread
publish_thread = Thread(target=self.publish_thread) #args=(self,))
publish_thread.start()
# number balls display
self.zero_balls = QPixmap(":/images/0_balls.png")
self.one_ball = QPixmap(":/images/1_ball.png")
self.two_balls = QPixmap(":/images/2_balls.png")
self.three_balls = QPixmap(":/images/3_balls.png")
self.four_balls = QPixmap(":/images/4_balls.png")
self.five_balls = QPixmap(":/images/5_balls.png")
self.more_than_five_balls = QPixmap(":/images/more_than_5_balls.png")
self.n_balls = -1 #don't know n balls at first
#in range stuff
self.shooter_in_range = False
self.turret_in_range = False
self.in_range_pixmap = QPixmap(":/images/GreenTarget.png")
self.not_in_range_pixmap = QPixmap(":/images/RedTarget.png")
self._widget.in_range_display.setPixmap(self.not_in_range_pixmap)
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
#initialize subscribers last, so that any callbacks they execute won't interfere with init
auto_state_listener = roslibpy.Topic(self.client, '/auto/auto_state', 'behavior_actions/AutoState')
self.auto_state_sub = auto_state_listener.subscribe(self.autoStateCallback)
n_balls_listener = roslibpy.Topic(self.client,'/num_powercells','std_msgs/UInt8')
self.n_balls_sub = n_balls_listener.subscribe(self.nBallsCallback)
shooter_in_range_listener = roslibpy.Topic(self.client, '/shooter/shooter_in_range', std_msgs.msg.Bool)
self.shooter_in_range_sub = shooter_in_range_listener.subscribe(self.shooterInRangeCallback)
turret_in_range_listener = roslibpy.Topic(self.client, '/align_to_shoot/turret_in_range', std_msgs.msg.Bool)
self.turret_in_range_sub = turret_in_range_listener.subscribe(self.turretInRangeCallback)
self.autoStateSignal.connect(self.autoStateSlot)
self.nBallsSignal.connect(self.nBallsSlot)
self.shooterInRangeSignal.connect(self.shooterInRangeSlot)
self.turretInRangeSignal.connect(self.turretInRangeSlot)
def autoStateCallback(self, msg):
self.autoStateSignal.emit(int(msg.id));
def autoStateSlot(self, state):
#self.lock.acquire()
if(self.autoState != state):
self.autoState = state
self.displayAutoState()
def displayAutoState(self):
if self.autoState == 0:
self._widget.auto_state_display.setText("Not ready")
self._widget.auto_state_display.setStyleSheet("background-color:#ff5555;")
elif self.autoState == 1:
self._widget.auto_state_display.setText("Ready, waiting for auto period")
self._widget.auto_state_display.setStyleSheet("background-color:#ffffff;")
elif self.autoState == 2:
self._widget.auto_state_display.setText("Running")
self._widget.auto_state_display.setStyleSheet("background-color:#ffff00")
elif self.autoState == 3:
self._widget.auto_state_display.setText("Finished")
self._widget.auto_state_display.setStyleSheet("background-color:#00ff00;")
elif self.autoState == 4:
self._widget.auto_state_display.setText("Error")
self._widget.auto_state_display.setStyleSheet("background-color:#ff5555;")
def nBallsCallback(self, msg):
self.nBallsSignal.emit(int(msg.data))
def nBallsSlot(self, state):
if(self.n_balls != state):
self.n_balls = state
display = self._widget.n_balls_display
if state == 0:
display.setPixmap(self.zero_balls)
elif state == 1:
display.setPixmap(self.one_ball)
elif state == 2:
display.setPixmap(self.two_balls)
elif state == 3:
display.setPixmap(self.three_balls)
elif state == 4:
display.setPixmap(self.four_balls)
elif state == 5:
display.setPixmap(self.five_balls)
elif state > 5:
display.setPixmap(self.more_than_five_balls)
else:
display.setText("Couldn't read # balls")
def shooterInRangeCallback(self, msg):
self.shooterInRangeSignal.emit(bool(msg.data))
def shooterInRangeSlot(self, in_range):
self.shooter_in_range = in_range #set here so that it's set synchronously with the other slots
def turretInRangeCallback(self, msg):
self.turretInRangeSignal.emit(bool(msg.data))
self.updateInRange()
def turretInRangeSlot(self, in_range):
self.turret_in_range = in_range #set here so that it's set synchronously with the other slots
self.updateInRange()
def updateInRange(self):
display = self._widget.in_range_display
if(self.shooter_in_range and self.turret_in_range):
display.setPixmap(self.in_range_pixmap)
else:
display.setPixmap(self.not_in_range_pixmap)
def setImuAngle(self):
print("setting imu")
#self.lock.acquire()
angle = self._widget.imu_angle.value() # imu_angle is the text field (doublespinbox) that the user can edit to change the navx angle, defaulting to zero
# call the service
try:
service = roslibpy.Service(self.client,'/imu/set_imu_zero',ImuZeroAngle)
# rospy.wait_for_service("/imu/set_imu_zero", 1) # timeout in sec, TODO maybe put in config file?
#TODO remove print
#Service Request-rosbridge
request = roslibpy.ServiceRequest()
result = service.call(request)
#result(angle)
# change button to green color to indicate that the service call went through
self._widget.set_imu_angle_button.setStyleSheet("background-color:#5eff00;")
except (rospy.ServiceException, rospy.ROSException) as e: # the second exception happens if the wait for service times out
self.errorPopup("Imu Set Angle Error", e)
#self.lock.release()
print("finished setting imu")
def imuAngleChanged(self):
#self.lock.acquire()
# change button to red color if someone fiddled with the angle input, to indicate that input wasn't set yet
self._widget.set_imu_angle_button
self._widget.set_imu_angle_button.setStyleSheet("background-color:#ff0000;")
#self.lock.release()
def setAutoWallDist(self):
print("setting auto wall distance")
#self.lock.acquire()
distance = self._widget.auto_wall_dist.value()
self._widget.auto_wall_dist_button.setStyleSheet("background-color:#5eff00;")
print("finished setting auto wall distance")
def autoWallDistChanged(self):
self._widget.auto_wall_dist_button.setStyleSheet("background-color:#ff0000;")
def resetBallCount(self):
print("manually reset ball count")
#self.lock.acquire()
nballs = self._widget.ball_reset_count.value()
# call the service
try:
rospy.wait_for_service("/reset_ball", 1) #timeout in sec, TODO maybe put in config file?
caller = rospy.ServiceProxy("/reset_ball", resetBallSrv)
caller(nballs)
# change button to green color to indicate that the service call went through
self._widget.set_imu_angle_button.setStyleSheet("background-color:##5eff00;")
except (rospy.ServiceException, rospy.ROSException) as e: # the second exception happens if the wait for service times out
self.errorPopup("Reset ball count Error", e)
#self.lock.release()
print("finished resetting ball count")
def resetBallChanged(self):
self._widget.ball_reset_button.setStyleSheet("background-color:#ff0000;")
def errorPopup(self, title, e):
msg_box = QMessageBox()
msg_box.setWindowTitle(title)
msg_box.setIcon(QMessageBox.Warning)
msg_box.setText("%s"%e)
msg_box.exec_()
#Publisher -> fake Auto States
def publish_thread(self):
pub = roslibpy.Topic(self.client, '/auto/auto_mode', 'behavior_actions/AutoMode')
r = rospy.Rate(10) # 10hz
pub.advertise()
while self.client.is_connected:
pub.publish(roslibpy.Message(
{
'auto_mode': self.auto_mode_button_group.checkedId()
}
))
r.sleep()
def shutdown_plugin(self):
self.auto_state_sub.unregister()
self.n_balls_sub.unregister()
self.shooter_in_range_sub.unregister()
self.turret_in_range_sub.unregister()
self.client.close()
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
pass
class SpectrogramPlugin(Plugin):
update_signal = QtCore.pyqtSignal()
subscriber_signal = QtCore.pyqtSignal(str)
module_num_signal = QtCore.pyqtSignal()
value_signal = QtCore.pyqtSignal(int)
vib_freq = 48000
overlap = 50
stft_freq = 200
frame_time = 1
frame_length = frame_time * stft_freq
label_flag = 0
lowcut_freq = 0
highcut_freq = vib_freq / 2
v_max = 0
v_min = 0
colorbarflag = 0
cnt_fig = 0
def __init__(self, context):
super(SpectrogramPlugin, self).__init__(context)
self.setObjectName('Spectrogram')
sns.set(style="whitegrid", palette="bright", color_codes=True)
self._widget = QWidget()
layout = QVBoxLayout()
self._widget.setLayout(layout)
layout_ = QHBoxLayout()
self.lbl_topic = QLabel('Topic:')
layout_.addWidget(self.lbl_topic)
self.le_topic = QLineEdit()
layout_.addWidget(self.le_topic)
self.apply_topic = QPushButton("Apply")
self.apply_topic.clicked.connect(self.apply_clicked_topic)
layout_.addWidget(self.apply_topic)
layout.addLayout(layout_)
layout_ = QHBoxLayout()
self.lbl_lcf = QLabel('Low-cut Freq.[Hz]:')
layout_.addWidget(self.lbl_lcf)
self.spb_lcf = QSpinBox()
self.spb_lcf.setRange(0, 50)
self.spb_lcf.setValue(0)
layout_.addWidget(self.spb_lcf)
self.apply_lcf = QPushButton("Apply")
self.apply_lcf.clicked.connect(self.apply_clicked_lcf)
layout_.addWidget(self.apply_lcf)
layout.addLayout(layout_)
layout_ = QHBoxLayout()
self.lbl_hcf = QLabel('High-cut Freq.[Hz]:')
layout_.addWidget(self.lbl_hcf)
self.spb_hcf = QSpinBox()
self.spb_hcf.setRange(50, self.vib_freq / 2)
self.spb_hcf.setValue(self.vib_freq / 2)
layout_.addWidget(self.spb_hcf)
self.apply_hcf = QPushButton("Apply")
self.apply_hcf.clicked.connect(self.apply_clicked_hcf)
layout_.addWidget(self.apply_hcf)
layout.addLayout(layout_)
#self.fig, self.axes = plt.subplots(2, 1, sharex=True)
self.fig = plt.figure()
self.ax = self.fig.add_subplot(1, 1, 1)
self.canvas = FigureCanvas(self.fig)
self.fig.tight_layout()
layout.addWidget(self.canvas)
context.add_widget(self._widget)
self.update_signal.connect(self.update_spectrogram)
self.subscriber_signal.connect(self.update_subscriber)
self.subscriber_signal.emit('spectrum')
def changed_spinbox_value(self, n):
self.valueChanged.emit(n)
def spectrum_callback(self, data):
nch = data.nch
len = data.nfreq
if self.spectrogram is None:
self.spectrogram = zeros([len, self.frame_length, nch])
#self.spectrogram = ones([len, self.frame_length, 4*nch])*log(1e-8)
self.spectrogram = roll(self.spectrogram, -1, 1)
for i in range(nch):
s = array(data.data).reshape([nch, len, 2])[i]
s = linalg.norm(s, axis=1)
s += 1e-8
log(s, s)
self.spectrogram[:, -1, i] = s
#if data.header.seq % 2 == 0:
if self.v_max < self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].max():
self.v_max = self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].max()
if self.v_min > self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].min():
self.v_min = self.spectrogram[self.lowcut_freq:self.highcut_freq,
-1, i].min()
self.update_signal.emit()
def apply_clicked_topic(self):
self.update_subscriber(self.le_topic.displayText())
def apply_clicked_lcf(self):
self.lowcut_freq = self.spb_lcf.value()
def apply_clicked_hcf(self):
self.highcut_freq = self.spb_hcf.value()
def update_spectrogram(self):
if self.spectrogram is not None:
yticks = [
0, self.highcut_freq / 2 / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq,
self.highcut_freq / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq,
self.highcut_freq / 2 * 3 / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq,
self.highcut_freq * 2 / self.stft_freq -
self.lowcut_freq * 2 / self.stft_freq
]
yticks_label = [
self.lowcut_freq, self.highcut_freq / 4, self.highcut_freq / 2,
self.highcut_freq / 4 * 3, self.highcut_freq
]
xticks = [
0, self.frame_length / 4 - 1, self.frame_length / 2 - 1,
self.frame_length * 3 / 4 - 1, self.frame_length - 1
]
xticks_label = [
-self.frame_time * 2, -self.frame_time / 2 * 3,
-self.frame_time, -self.frame_time / 2, 0
]
font_size = 13
if self.cnt_fig % 40 == 0:
self.ax.clear()
#self.im = self.ax.imshow(self.spectrogram[int(self.lowcut_freq*100/self.overlap/self.stft_freq):int(self.highcut_freq*100/self.overlap/self.stft_freq)+1,:,0], aspect="auto", origin="lower", cmap="winter", interpolation='none', vmin=self.v_min, vmax=self.v_max)
self.im = self.ax.imshow(self.spectrogram[
int(self.lowcut_freq * 100 / self.overlap /
self.stft_freq):int(self.highcut_freq * 100 /
self.overlap / self.stft_freq) +
1, :, 0],
aspect="auto",
origin="lower",
cmap="jet",
interpolation='none',
vmin=12,
vmax=16)
self.ax.grid(None)
self.ax.set_ylabel("Freq. [Hz]",
fontsize=font_size,
fontname='serif')
self.ax.set_yticks(yticks)
self.ax.set_yticklabels(["$%.1f$" % y for y in yticks_label],
fontsize=font_size)
self.ax.set_xticks(xticks)
self.ax.set_xticklabels(["$%.1f$" % x for x in xticks_label],
fontsize=font_size)
self.ax.set_xlabel("Time [s]",
fontsize=font_size,
fontname='serif')
if self.colorbarflag == 0:
self.colorbarflag = 1
self.cb = self.fig.colorbar(self.im)
elif self.colorbarflag == 1:
self.cb.update_bruteforce(self.im)
self.canvas.draw()
self.cnt_fig += 1
QApplication.processEvents()
def update_subscriber(self, topic_name):
self.topic_name = topic_name
self.le_topic.setText(self.topic_name)
if hasattr(self, 'sub'):
self.sub.unregister()
self.spectrogram = None
#self.topic_name = 'spectrum'
self.sub = rospy.Subscriber(topic_name,
Spectrum,
self.spectrum_callback,
queue_size=5)
def shutdown_plugin(self):
pass
class ManualWindow(QtWidgets.QDialog):
"""doc"""
activate = QtCore.pyqtSignal(bool)
stop_camera = QtCore.pyqtSignal()
stop_pose = QtCore.pyqtSignal()
stop_serial = QtCore.pyqtSignal()
stop_command = QtCore.pyqtSignal()
stop_xbox_controller = QtCore.pyqtSignal()
change_camera = QtCore.pyqtSignal(bool)
def __init__(self):
super(ManualWindow, self).__init__()
self.setAttribute(QtCore.Qt.WA_DeleteOnClose)
self.ui = '../forms/manual.ui'
loadUi(self.ui, self)
self.setWindowFlags(QtCore.Qt.FramelessWindowHint)
self.layout = self.findChild(QtWidgets.QGridLayout, 'layout')
self.mode = JOYSTICK_ONLY_MODE
self.visual_frame = rviz.VisualizationFrame()
self.visual_frame.setSplashPath("")
self.visual_frame.initialize()
self.add_rviz_config()
self.visual_frame.setMenuBar(None)
self.visual_frame.setStatusBar(None)
self.visual_frame.setHideButtonVisibility(False)
self.layout.addWidget(self.visual_frame)
self.visual_frame.hide()
self.manager = self.visual_frame.getManager()
self.grid_display = self.manager.getRootDisplayGroup().getDisplayAt(0)
self.video_frame = QtWidgets.QLabel()
video_frame_font = QtGui.QFont("Verdana", 62, QtGui.QFont.Bold)
self.video_frame.setFont(video_frame_font)
self.video_frame.setText("No video frame")
self.video_frame.setAlignment(QtCore.Qt.AlignCenter)
self.video_frame.hide()
self.layout.addWidget(self.video_frame)
self.joystick_frame = QtWidgets.QLabel()
pixmap = QtGui.QPixmap('../static/img/xbox_controller_grey.png')
pixmap = pixmap.scaledToWidth(800)
self.joystick_frame.setPixmap(pixmap)
self.joystick_frame.setAlignment(QtCore.Qt.AlignCenter)
self.layout.addWidget(self.joystick_frame)
# Buttons
self.change_mode_btn = self.findChild(QtWidgets.QPushButton,
'changeModeBtn')
self.turn_left = self.findChild(QtWidgets.QToolButton, 'turnRobotLeft')
self.turn_right = self.findChild(QtWidgets.QToolButton,
'turnRobotRight')
self.stand_btn = self.findChild(QtWidgets.QPushButton, 'standBtn')
self.walk_btn = self.findChild(QtWidgets.QPushButton, 'walkBtn')
self.stairs_btn = self.findChild(QtWidgets.QPushButton, 'stairsBtn')
self.exit_btn = self.findChild(QtWidgets.QPushButton, 'exitBtn')
self.powerBtn = self.findChild(QtWidgets.QPushButton, 'powerBtn')
self.emergency_btn = self.findChild(QtWidgets.QPushButton,
'emergencyBtn')
# Status indicators
self.signal_btn = self.findChild(QtWidgets.QPushButton, 'signalBtn')
self.controller_battery_btn = self.findChild(QtWidgets.QPushButton,
'controllerBatteryBtn')
self.battery_btn = self.findChild(QtWidgets.QPushButton, 'batteryBtn')
self.health_btn = self.findChild(QtWidgets.QPushButton, 'healthBtn')
# Button connections
self.emergency_btn.clicked.connect(self.turn_robot_off)
self.powerBtn.clicked.connect(self.power_on)
self.exit_btn.clicked.connect(self.close_window)
self.change_mode_btn.clicked.connect(self.change_mode)
# Button shortcuts
self.exit_btn.setShortcut("Ctrl+Q")
# Mode Layouts
#self.video_frame = self.findChild(QtWidgets.QLabel, 'videoFrame')
# self.xbox_controller_frame = self.findChild(
# QtWidgets.QLabel, 'xboxcontrollerFrame')
# self.video_frame.hide()
# Stylesheets
self.powerBtn.setStyleSheet(
"QPushButton#powerBtn:checked {color:black; background-color: red;}"
)
self.signal_btn.setStyleSheet(
"QPushButton#signalBtn:checked {color:black; background-color: green;}"
)
self.video_stream_subscriber = VideoStreamSubscriber(
'camera/image_raw', Image)
self.show()
def add_rviz_config(self):
reader = rviz.YamlConfigReader()
config = rviz.Config()
reader.readFile(config, "../instance/Sparkie.rviz")
self.visual_frame.load(config)
def change_mode(self):
if self.mode < MAX_MODES:
self.mode += 1
else:
self.mode = JOYSTICK_ONLY_MODE
self.update_mode_label()
def update_mode_label(self):
if self.mode == 0:
self.change_mode_btn.setText("JOYSTICK")
self.visual_frame.hide()
self.joystick_frame.show()
elif self.mode == 1:
self.change_mode_btn.setText("CAMERAS")
self.joystick_frame.hide()
self.video_frame.show()
elif self.mode == 2:
self.change_mode_btn.setText("VISUAL")
self.video_frame.hide()
self.visual_frame.show()
def power_on(self):
active = self.powerBtn.isChecked()
if active:
self.activate.emit(True)
else:
self.activate.emit(False)
def blink_connection(self):
if not self.signal_btn.isChecked():
self.signal_btn.setChecked(True)
else:
self.signal_btn.setChecked(False)
def close_window(self):
self.close()
def turn_robot_off(self):
pass
class UpdateThread(QtCore.QThread):
""" Update thread """
description = QtCore.pyqtSignal(['QString'])
spec = QtCore.pyqtSignal(['QString'])
key = QtCore.pyqtSignal(['QString'])
buttons = QtCore.pyqtSignal(['QString']) # String, buttons separated by ;
valid = QtCore.pyqtSignal([bool])
def __init__(self, gui, server):
""" Constructor
:param gui: GUI where things are displayed
:param server: hmi server interface
"""
super(UpdateThread, self).__init__()
print "Thread init"
self._gui = gui
self._server = server
self._description = ""
self._spec = ""
self._buttons = ""
self._key = ""
self._valid = False
self._submit = False
self._current_text = ""
def run(self):
print "Thread.run"
r = rospy.Rate(5)
while not rospy.is_shutdown():
# question, buttons = self._server.update(self._current_text)
res = self._server.update(current_text=self._current_text, submit=self._submit)
# if question and question != self.question:
if res.description != self._description:
self.description.emit(res.description)
self._description = res.description
if res.key != self._key:
self.key.emit(res.key)
self._key = res.key
if res.spec != self._spec:
self.spec.emit(res.spec)
self._spec = res.spec
buttonstr = ""
for b in res.buttons:
buttonstr += (b + ";")
if buttonstr != self._buttons:
self.buttons.emit(buttonstr)
self._buttons = buttonstr
if res.valid != self._valid:
self.valid.emit(res.valid)
self._valid = res.valid
self._submit = False # Reset submit button
r.sleep()
def set_text(self, text):
""" Sets the text to use in the update loop of the HMI server
:param text: text to set
"""
self._current_text = str(text)
def submit(self):
""" Slot for the submit button of the GUI
"""
self._submit = True
class QtScan(QtGui.QWidget):
accepted = QtCore.pyqtSignal(list)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
loadUi(os.path.join(dirname, 'resources', 'scan.ui'), self)
rospy.Subscriber('/ueye/scan',
PointCloud2,
self.cbPointCloud,
queue_size=1)
# rospy.Subscriber(
# '/supervisor/status', MsgStatus, self.cbStatus, queue_size=1)
rospy.Subscriber('/ueye/zheight',
std_msgs.msg.Float32,
self.cbHeight,
queue_size=1)
self.pub_marker_array = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=10)
self.btnPlane.clicked.connect(self.btnPlaneClicked)
self.btnRecord.clicked.connect(self.btnRecordClicked)
self.btnZmap.clicked.connect(self.btnZmapClicked)
self.btnPath.clicked.connect(self.btnPathClicked)
self.btnScan.clicked.connect(self.btnScanClicked)
self.sbPositionX.valueChanged.connect(self.changePosition)
self.sbPositionY.valueChanged.connect(self.changePosition)
self.sbPositionZ.valueChanged.connect(self.changePosition)
self.sbSizeX.valueChanged.connect(self.changeSize)
self.sbSizeY.valueChanged.connect(self.changeSize)
self.sbSizeZ.valueChanged.connect(self.changeSize)
self.filename = ''
self.status = False
self.running = False
self.recording = False
self.path = []
self.scan_markers = None
self.planning = Planning()
self.position = np.array([0, 0, 10])
self.size = np.array([100, 200, 0])
self.segmentation = Segmentation()
self.zmax = 0.0
self.new_h = False
rospy.Timer(rospy.Duration(5), self.timer_callback)
def timer_callback(self, event):
if self.new_h:
self.new_h = False
else:
self.lcdZ.setEnabled(False)
def cbHeight(self, msg_float):
self.lcdZ.display(float(msg_float.data * 1000.0))
self.lcdZ.setEnabled(True)
def cbPointCloud(self, msg_cloud):
points = pc2.read_points(msg_cloud, skip_nans=False)
self.points3d = np.float32([point for point in points])
if self.recording:
print self.filename
with open(self.filename, 'a') as f:
np.savetxt(f, self.points3d, fmt='%.6f')
def cbStatus(self, msg_status):
status = msg_status.running
if not self.status and status:
if self.running:
self.recording = True
self.btnRecord.setText('Recording...')
elif self.status and not status:
self.running = False
self.recording = False
self.btnRecord.setText('Record Cloud')
self.status = status
def updatePlane(self):
(x, y, z), (w, h, t) = self.position, self.size
plane = np.array([[x, y, z], [x + w, y, z], [x + w, y + h, z],
[x, y + h, z]])
slice = [plane - np.array([0, 50, 0])] # laser stripe offset
path = self.planning.get_path_from_slices([slice],
track_distance=100,
focus=100)
self.path = [[pos, ori] for pos, ori, bol in path]
self.scan_markers.set_plane_size(self.size)
self.scan_markers.set_plane_position(self.position)
self.scan_markers.set_path(self.path)
self.pub_marker_array.publish(self.scan_markers.marker_array)
def changePosition(self):
self.position = np.array([
self.sbPositionX.value(),
self.sbPositionY.value(),
self.sbPositionZ.value()
])
self.updatePlane()
def changeSize(self):
self.size = np.array(
[self.sbSizeX.value(),
self.sbSizeY.value(),
self.sbSizeZ.value()])
self.updatePlane()
def btnPlaneClicked(self):
self.scan_markers = ScanMarkers()
self.updatePlane()
def btnRecordClicked(self):
if self.running:
self.running = False
self.recording = False
self.btnRecord.setText('Record Cloud')
else:
try:
filename = QtGui.QFileDialog.getSaveFileName(
self, 'Save file', os.path.join(dirname, 'data',
'test.xyz'),
'Point Cloud Files (*.xyz)')[0]
self.filename = filename
with open(self.filename, 'w') as f:
pass
self.running = True
self.recording = True
self.btnRecord.setText('Stop recording...')
except IOError as error:
print error
def btnZmapClicked(self):
filename = QtGui.QFileDialog.getOpenFileName(
self, 'Load file', os.path.join(dirname, 'data', 'test.xyz'),
'Point Cloud Files (*.xyz)')[0]
name, extension = os.path.splitext(filename)
if os.path.isfile(filename):
cloud = contours.read_cloud(filename)
self.zmap = contours.zmap_from_cloud(cloud)
self.zmap = contours.fill_zmap(self.zmap, size=7)
self.zmap = contours.erode_zmap(self.zmap, size=5, iterations=5)
contours.save_zmap('%s.tif' % name, self.zmap)
self.segmentation.plot_zmap(self.zmap)
else:
print 'Create Zmap: Incorrect filename'
def btnPathClicked(self):
try:
slice = contours.slice_of_contours(self.zmap,
self.segmentation.contours)
path = self.planning.get_path_from_slices([slice],
track_distance=1.3,
focus=0)
#contours.show_path_from_slice(slice)
self.path = path
#self.path = [[pos, ori] for pos, ori, bol in path]
self.scan_markers.set_path(self.path)
self.pub_marker_array.publish(self.scan_markers.marker_array)
except AttributeError as error:
print error
def btnScanClicked(self):
self.accepted.emit(self.path)
class HardwareUI(Plugin):
"""Backend for the GUI. Receives UDP data and distributes it across all GUI elements."""
tetrigger = QtCore.pyqtSignal()
totrigger = QtCore.pyqtSignal()
votrigger = QtCore.pyqtSignal()
imutrigger = QtCore.pyqtSignal(Imu)
def __init__(self, context):
"""Loads up the master.ui GUI file and adds some additional widgets that cannot be added beforehand. Also sets some necessary variables."""
super(HardwareUI, self).__init__(context)
self._widget = QMainWindow()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('bitbots_hardware_rqt'), 'resource',
'master.ui')
loadUi(ui_file, self._widget, {})
self.current_tab = 0
self.current_outer_tab = 0
self.historydict = []
self.colorlist = [
] #Holds information about the graph colors, so we can distinguish between the different motors
self.motornames = [
'HeadPan', 'HeadTilt', 'LAnklePitch', 'LAnkleRoll', 'LElbow',
'LHipPitch', 'LHipRoll', 'LHipYaw', 'LKnee', 'LShoulderPitch',
'LShoulderRoll', 'RAnklePitch', 'RAnkleRoll', 'RElbow',
'RHipPitch', 'RHipRoll', 'RHipYaw', 'RKnee', 'RShoulderPitch',
'RShoulderRoll'
] #We need the motor names in alphabetical order for some gui elements
for i in range(0, 20):
self.colorlist.append(
(random.randrange(50, 255), random.randrange(0, 255),
random.randrange(0, 255)))
self.templist = [[] for i in range(20)]
self.torquelist = [[] for i in range(20)]
self.voltagelist = [[] for i in range(20)]
self.crosshair = QLabel()
self.pixmap = QtGui.QPixmap(
os.path.join(rp.get_path('bitbots_hardware_rqt'), 'resource',
'cross.png'))
self.crosshair.setPixmap(self.pixmap)
self.crosshair.setParent(self._widget.label_4)
self.crosshair.move(187, 187)
self.crosshair2 = QLabel()
self.pixmap = QtGui.QPixmap(
os.path.join(rp.get_path('bitbots_hardware_rqt'), 'resource',
'cross.png'))
self.crosshair2.setPixmap(self.pixmap)
self.crosshair2.setParent(self._widget.label_11)
self.crosshair2.move(87, 87)
self.make_Graphs()
self.votrigger.connect(lambda: self.update_graph(
self.voltageplt, self.voltagelist, self.combobox2, 3))
self.tetrigger.connect(lambda: self.update_graph(
self.tempplt, self.templist, self.combobox3, 1))
self.totrigger.connect(lambda: self.update_graph(
self.torqueplt, self.torquelist, self.combobox, 2))
#self._robot_ip = [l for l in ([ip for ip in socket.gethostbyname_ex(socket.gethostname())[2] if not ip.startswith("127.")][:1], [[(s.connect(('8.8.8.8', 53)), s.getsockname()[0], s.close()) for s in [socket.socket(socket.AF_INET, socket.SOCK_DGRAM)]][0][1]]) if l][0][0]
self._robot, button_pressed = QInputDialog.getText(self._widget, "Robot?", \
"Select your Robot. Example: nuc1", QLineEdit.Normal, "")
self.diagnostics = rospy.Subscriber(
"/{}/diagnostics".format(self._robot), DiagnosticArray,
self.set_motor_diagnostics)
self.joint_states = rospy.Subscriber(
"/{}/joint_states".format(self._robot), JointState,
self.set_motor_joint_states)
self.buttons = rospy.Subscriber("/{}/buttons".format(self._robot),
Buttons, self.set_buttons)
self.imu = rospy.Subscriber("/{}/imu/data".format(self._robot), Imu,
self.emit_imu_trigger)
self.state = rospy.Subscriber("/{}/robot_state".format(self._robot),
RobotControlState, self.set_robot_state)
self.imutrigger.connect(self.set_imu)
#self.rcvthread = ReceiverThread(self._robot_ip, self._robot_port)
#self.rcvthread.start()
self._widget.GraphenView.currentChanged.connect(
self.current_graph_changed)
self._widget.tabWidget.currentChanged.connect(
self.current_outer_tab_changed)
#self._widget.label_14.setText("IP: "+ [l for l in ([ip for ip in socket.gethostbyname_ex(socket.gethostname())[2] if not ip.startswith("127.")][:1], [[(s.connect(('8.8.8.8', 53)), s.getsockname()[0], s.close()) for s in [socket.socket(socket.AF_INET, socket.SOCK_DGRAM)]][0][1]]) if l][0][0])
context.add_widget(self._widget)
self._widget.show()
def calcPosInCicle(self, radOrienation):
"""return a position on the circle in the GUI"""
yPos = -math.cos(radOrienation) * 100 + 87
xPos = math.sin(radOrienation) * 100 + 87
return xPos, yPos
def current_graph_changed(self, change):
"""Handles the changing between tabs, so that only the active tab is updated, to reduce lag"""
self.current_tab = change
def current_outer_tab_changed(self, change):
self.current_outer_tab = change
def emit_imu_trigger(self, data):
self.imutrigger.emit(data)
def set_imu(self, data):
"""Updates the IMU/Gyro widgets."""
if self.current_outer_tab == 2:
self._widget.label_8.setText(
str(math.degrees(data.angular_velocity.x)))
self._widget.label_9.setText(
str(math.degrees(data.angular_velocity.y)))
self._widget.label_10.setText(
str(math.degrees(data.angular_velocity.z)))
self.crosshair.move((187+(math.degrees(data.angular_velocity.x)*400/360)), \
(187+(math.degrees(data.angular_velocity.y)*400/360))) #To place the crosshair on the circle, circle diametre is 400px, crosshair is 25px wide
yawx, yawy = self.calcPosInCicle(data.angular_velocity.z)
self.crosshair2.move(yawx, yawy)
def set_buttons(self, data):
"""Updates the widgets placed in the topbar"""
self._widget.checkBox.setCheckState(data.button1)
self._widget.checkBox.setCheckState(data.button2)
def set_robot_state(self, data):
states = {
0: "CONTROLLABLE",
1: "FALLING",
2: "FALLEN",
3: "GETTING UP",
4: "ANIMATION RUNNING",
5: "STARTUP",
6: "SHUTDOWN",
7: "PENALTY",
8: "PENALTY ANIM",
9: "RECORD",
10: "WALKING",
11: "MOTOR OFF",
12: "HCM OFF",
13: "HARDWARE PROBLEM",
14: "PICKED UP"
}
self._widget.label_14.setText(states[data.state])
def set_motor_diagnostics(self, data):
"""Updates the table in the motor overview tab"""
self.timestamp = data.header.stamp.secs #setting timestamp here, because this topic should always exist.
self._widget.label_13.setText(str(self.timestamp))
self.motorheaderlist = []
for i in range(len(data.status)):
if data.status[i].level == 1:
self.templist[int(data.status[i].hardware_id) - 101].append(
float(data.status[i].values[3].value))
self.voltagelist[int(data.status[i].hardware_id) - 101].append(
float(data.status[i].values[1].value))
if len(self.templist[i]) > 20:
self.templist[i].pop(0)
if len(self.voltagelist[i]) > 20:
self.voltagelist[i].pop(0)
if self.current_tab == 0:
if data.status[i].level == 1:
if not data.status[i].values[
2].value in self.motorheaderlist:
self.motorheaderlist.append(
data.status[i].values[2].value)
for j in range(0, len(self.motorheaderlist)):
if j < 10:
selected = self._widget.tableWidget
k = j
else:
selected = self._widget.tableWidget_2
k = j - 10
try:
if self.motorheaderlist[j] == data.status[
i].values[2].value:
selected.setItem(
0, k,
QTableWidgetItem(
data.status[i].message))
selected.setItem(
1, k,
QTableWidgetItem(
str(
round(
float(data.status[i].
values[3].value),
2))))
selected.setItem(
3, k,
QTableWidgetItem(
str(
round(
float(data.status[i].
values[1].value),
2))))
selected.setItem(
4, k,
QTableWidgetItem(
data.status[i].values[0].value))
except IndexError:
rospy.logwarn(
"Received a message that misses some information. Faulty message: "
+ data.status[i])
self._widget.tableWidget.setHorizontalHeaderLabels(
self.motorheaderlist)
self._widget.tableWidget_2.setHorizontalHeaderLabels(
self.motorheaderlist[10:])
self._widget.tableWidget.reset()
self._widget.tableWidget_2.reset()
self._widget.tableWidget.setEditTriggers(
QTableWidget.NoEditTriggers)
self.tetrigger.emit()
self.votrigger.emit()
def set_motor_joint_states(self, data):
if self.current_tab == 0:
self.motorheaderlist = []
for i in range(0, len(data.name)):
self.torquelist[i].append(float(data.effort[i]))
if not data.name[i] in self.motorheaderlist:
self.motorheaderlist.append(data.name[i])
for j in range(0, len(self.motorheaderlist)):
if j < 10:
selected = self._widget.tableWidget
k = j
else:
selected = self._widget.tableWidget_2
k = j - 10
for j in self.motorheaderlist:
if j == data.name[i]:
selected.setItem(
2, k,
QTableWidgetItem(
str(
round(
float(
math.degrees(
data.position[i])), 2))))
selected.setItem(
5, k,
QTableWidgetItem(
str(round(float(data.effort[i]), 2))))
self._widget.tableWidget.setHorizontalHeaderLabels(
self.motorheaderlist)
self._widget.tableWidget_2.setHorizontalHeaderLabels(
self.motorheaderlist[10:])
self.totrigger.emit()
def update_graph(self, graph, glist, cbox, number):
"""Updates the graph that displays the motors temperature"""
if self.current_tab == number:
graph.clear()
graph.setXRange(-1, 21)
for i in range(len(glist)):
if cbox.currentText() == 'All' or cbox.currentIndex() - 1 == i:
path = pg.arrayToQPath(np.arange(len(glist[i])), glist[i])
item = QtGui.QGraphicsPathItem(path)
item.setPen(pg.mkPen(color=self.colorlist[i]))
graph.addItem(item)
def make_Graphs(self):
"""Method to initialize the different plots.
Creates a plot with grid and legend, then adds it to the coresponding layout.
"""
self.tempplt = pg.plot()
self.tempplt.setYRange(-1, 100)
self.tempplt.showGrid(x=True, y=True)
self.tempplt.addLegend()
for i in range(0, 10):
self.tempplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.tempplt.addLegend(offset=79)
#creates a legend by plotting a single point for each motor, just so they show up in the legend.
#Apparently you are supposed to do it that way...
for i in range(10, 20):
self.tempplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.layout_temp = QtGui.QHBoxLayout()
self.combobox3 = QComboBox()
self.combobox3.addItem('All')
for i in self.motornames:
self.combobox3.addItem(i)
self._widget.Temperatur.setLayout(self.layout_temp)
self.layout_temp.addWidget(self.tempplt)
self.layout_temp.addWidget(self.combobox3)
self.tempplt.win.hide()
self.torqueplt = pg.plot()
self.torqueplt.setYRange(-1, 2)
self.torqueplt.showGrid(x=True, y=True)
self.torqueplt.addLegend()
for i in range(0, 10):
self.torqueplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.torqueplt.addLegend(offset=79)
for i in range(10, 20):
self.torqueplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.layout_torque = QtGui.QHBoxLayout()
self._widget.Torque.setLayout(self.layout_torque)
self.layout_torque.addWidget(self.torqueplt)
self.combobox = QComboBox()
self.combobox.addItem('All')
for i in self.motornames:
self.combobox.addItem(i)
self.layout_torque.addWidget(self.combobox)
self.torqueplt.win.hide()
self.voltageplt = pg.plot()
self.voltageplt.setYRange(-1, 30)
self.voltageplt.showGrid(x=True, y=True)
self.voltageplt.addLegend()
for i in range(0, 10):
self.voltageplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.voltageplt.addLegend(offset=79)
for i in range(10, 20):
self.voltageplt.plot(np.zeros(10),
np.zeros(10),
pen=self.colorlist[i],
name=self.motornames[i])
self.layout_voltage = QtGui.QHBoxLayout()
self.combobox2 = QComboBox()
self.combobox2.addItem('All')
for i in self.motornames:
self.combobox2.addItem(i)
self._widget.Voltage.setLayout(self.layout_voltage)
self.layout_voltage.addWidget(self.voltageplt)
self.layout_voltage.addWidget(self.combobox2)
self.voltageplt.win.hide()
class ContinueGui(QtGui.QWidget):
"""
GUI for the RoboCup continue rule
"""
current_text = QtCore.pyqtSignal(['QString']) # Necessary to emit current text to update thread
def __init__(self, grammar_filename=None):
""" Constructor """
super(ContinueGui, self).__init__()
self.resize(640, 480)
self.move(300, 300)
self.setWindowTitle('Continue GUI')
# Add the description box
self.description_label = QtGui.QLabel(self)
self.description_label.setMaximumHeight(100)
# Add the question box
self.spec_label = QtGui.QLabel(self)
self.spec_label.setMaximumHeight(100)
# Add the key box
self.key_label = QtGui.QLabel(self)
self.key_label.setMaximumHeight(100)
# Add the texteditbox
self.textbox = QtGui.QTextEdit(self)
self.textbox.textChanged.connect(self._text_changed)
# Add the submit button
self.submit_button = QtGui.QPushButton(self)
self.submit_button.setText("Submit")
self.submit_button.setEnabled(False)
# Add the clear button
self.clear_button = QtGui.QPushButton(self)
self.clear_button.setText("Clear")
# Submit/clear button layout
self.submit_clear_button_layout = QtGui.QVBoxLayout()
self.submit_clear_button_layout.addWidget(self.submit_button)
self.submit_clear_button_layout.addWidget(self.clear_button)
self.submit_clear_button_widget = QtGui.QWidget()
self.submit_clear_button_widget.setLayout(self.submit_clear_button_layout)
# Add the QButtonGroup
# self.option_buttons = QtGui.QButtonGroup(self) (ToDo: how can I do this nicely???)
# self.button_layout = QtGui.QVBoxLayout()
self.button_layout = QtGui.QGridLayout()
self.button_widget = QtGui.QWidget()
self.button_widget.setLayout(self.button_layout)
self.button_widget.setSizePolicy(2, 2) # Makes sure we don't get enormous buttons
# Top layout
self._top_layout = QtGui.QHBoxLayout(self)
self._top_layout.addWidget(self.textbox)
self._top_layout.addWidget(self.submit_clear_button_widget)
self._top_widget = QtGui.QWidget()
self._top_widget.setLayout(self._top_layout)
self._top_widget.setMaximumHeight(100)
# Main layout
self._main_layout = QtGui.QVBoxLayout(self)
self._main_layout.addWidget(self.description_label)
self._main_layout.addWidget(self.spec_label)
self._main_layout.addWidget(self.key_label)
self._main_layout.addWidget(self._top_widget)
# self._main_layout.addWidget(self.option_buttons)
self._main_layout.addWidget(self.button_widget)
self._main_layout.insertStretch(-1, 0)
# List with ButtonCBs
self._button_cbs = []
# Setup the interface
self.server_interface = HMIServerGUIInterface('continui', grammar_filename)
# Update thread
self.update_thread = UpdateThread(self, self.server_interface)
self.update_thread.buttons.connect(self.buttons_callback)
self.update_thread.description.connect(self.description_callback)
self.update_thread.spec.connect(self.spec_callback)
self.update_thread.key.connect(self.key_callback)
self.update_thread.valid.connect(self.valid_callback)
self.current_text.connect(self.update_thread.set_text)
self.submit_button.clicked.connect(self.update_thread.submit)
self.submit_button.clicked.connect(self.clear_text)
self.clear_button.clicked.connect(self.clear_text)
self.update_thread.start()
self.show()
def _clear_clicked(self):
print "bla"
def _text_changed(self):
""" Callback function if text in the textbox has changed. Gets the text and emits
this as a signal
"""
text = self.textbox.toPlainText()
self.current_text.emit(text)
def description_callback(self, text):
""" Puts the provided text in the label
:param text: string with text
"""
self.description_label.setText(text)
def key_callback(self, text):
""" Puts the provided text in the label
:param text: string with text
"""
self.key_label.setText(text)
def spec_callback(self, text):
""" Puts the provided text in the label
:param text: string with text
"""
self.spec_label.setText(text)
def valid_callback(self, valid):
""" Sets the 'Submit' button to enabled/disabled based on the valid
:param valid: bool whether this is valid
"""
self.submit_button.setEnabled(valid)
def add_to_textbox(self, text):
""" Adds the text to the textbox, taking spaces into account
:param text: text to add
:return:
"""
current_text = self.textbox.toPlainText()
if current_text == "":
self.textbox.insertPlainText(text)
else:
self.textbox.insertPlainText(" "+text)
def buttons_callback(self, buttons):
""" PyQt slot for buttons to add. Buttons are always cleared beforehand
:param buttons: QString containing the text of the buttons to add, separated by ';'
"""
# Start by clearing
self.clear_buttons()
# If we don't have to add anything, return
if not buttons:
return
start = rospy.Time.now()
# We want a maximum of 160 characters on one line. This means one word can have up to 160/5=32 characters
# If this is exceeded, we will scale accordingly
buttonlist = buttons.split(';')
max_length = len(buttonlist[0])
for b in buttonlist:
if len(b) > max_length:
max_length = len(b)
factor = 32.0/max_length
nr_cols = min(5, max(1, math.floor(factor * 5)))
nr_rows = math.ceil(len(buttons)/float(nr_cols))
row = 0
col = 0
for b in str(buttons).split(';'):
self.add_button(b, row, col)
col += 1
if col == nr_cols:
col = 0
row += 1
rospy.loginfo("Adding buttons took {0} seconds".format((rospy.Time.now() - start).to_sec()))
def add_button(self, text, row, col):
""" Adds a button with an option to the GUI
:param text: text of the button to add
:param row: row where to add the button
:param col: column where to add the button
"""
if text == "":
return
# self.clear_buttons()
b = QtGui.QPushButton()
b.setText(text.replace("_", " "))
bcb = ButtonCB(self, text)
self._button_cbs.append(bcb)
self.button_layout.addWidget(b, row, col)
b.clicked.connect(bcb.callback)
def clear_buttons(self):
""" Removes all option buttons """
# Remove all buttons
while True:
b = self.button_layout.takeAt(0)
if b is None:
break
else:
b.widget().setParent(None)
# Clear the list with callbacks
self._button_cbs = []
def clear_text(self):
""" Clears the text from the textbox """
self.textbox.setText("")
def closeEvent(self, event):
rospy.loginfo('Close event received')
rospy.signal_shutdown('close event')
