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')