def getUndefLabel(self, colorStr): """ initiates the undefined label :param colorStr: color as string :return: """ ls = self.pool.get(colorStr) if ls == []: pxmap = QPixmap(self._getUndefFile(colorStr)) undf = QLabel(parent=self.frame) undf.setPixmap(pxmap) undf.setScaledContents(True) undf.setFixedSize(self.size, self.size) undf.show() return undf else: undf = ls.pop() undf.show() return undf
def getBallLabel(self, colorStr): """ sets the ball label :param colorStr: the color fo the ball as string :return: """ ls = self.pool.get(colorStr) if ls == []: pxmap = QPixmap(self._getBallFile(colorStr)) ball = QLabel(parent=self.frame) ball.setPixmap(pxmap) ball.setScaledContents(True) ball.setFixedSize(self.size, self.size) ball.show() return ball else: ball = ls.pop() ball.show() return ball
def getTeammateLabel(self, colorStr): """ initaties the teammate label for the quarter field :param colorStr: color as string :return: """ ls = self.pool.get(colorStr) if ls == []: pxmap = QPixmap(self._getTeammateFile(colorStr)) mate = QLabel(parent=self.frame) mate.setPixmap(pxmap) mate.setScaledContents(True) mate.setFixedSize(self.size, self.size) mate.show() return mate else: mate = ls.pop() mate.show() return mate
def getOpponentLabel(self, colorStr): """ initiates the opponent label :param colorStr: color as string :return: """ ls = self.pool.get(colorStr) if ls == []: pxmap = QPixmap(self._getOpponentFile(colorStr)) opp = QLabel(parent=self.frame) opp.setPixmap(pxmap) opp.setScaledContents(True) opp.setFixedSize(self.size, self.size) opp.show() return opp else: opp = ls.pop() opp.show() return opp
def getCrossLabel(self, colorStr): """ initiates the cross label :param colorStr: color as string :return: """ ls = self.pool.get(colorStr) if ls == []: pxmap = QPixmap(self._getCrossFile(colorStr)) crs = QLabel(parent=self.frame) crs.setPixmap(pxmap) crs.setScaledContents(True) crs.setFixedSize(self.size, self.size) crs.show() return crs else: crs = ls.pop() crs.show() return crs
def getRobotLabel(self, colorStr): """ initiates the robot label :param colorStr: color as string :return: """ ls = self.pool.get(colorStr) if ls == []: pxmap = QPixmap(self._getRobFile(colorStr)) rob = QLabel(parent=self.frame) rob.setPixmap(pxmap) rob.originalPixmap = QPixmap(pxmap) rob.setScaledContents(True) rob.setFixedSize(self.size, self.size) rob.show() return rob else: rob = ls.pop() rob.show() return rob
class MyPlugin(Plugin): def __init__(self, context): super(MyPlugin, self).__init__(context) Black = np.zeros((300, 400, 3), np.uint8) self.is_decay_auto = 0 self.is_frame_auto = 0 self.is_segsz_auto = 0 self.bridge = CvBridge() self.d_image_sub = rospy.Subscriber("/fome/direction", Image, self.fome_dir) self.m_image_sub = rospy.Subscriber("/fome/magnitude", Image, self.fome_mag) self.c_image_sub = rospy.Subscriber("/fome/cluster", Image, self.fome_cls) # self.r_image_sub = rospy.Subscriber("/dvs_rendering",Image,self.fome_raw) self.r_image_sub = rospy.Subscriber("/dvs/image_raw", Image, self.fome_raw) self.decay_pub = rospy.Publisher("/fome/decay_rate", String, queue_size=1) self.frame_pub = rospy.Publisher("/fome/frame_rate", String, queue_size=1) self.segsz_pub = rospy.Publisher("/fome/segment_size", String, queue_size=1) self.setObjectName('fome_gui') rp = rospkg.RosPack() self._widget = QWidget() ui_file = os.path.join(rp.get_path('fome_gui'), 'resource', 'MyPlugin.ui') loadUi(ui_file, self._widget) self._widget.setObjectName('fome_gui') height, width, byteValue = Black.shape byteValue = byteValue * width self.d_image = QImage(Black, width, height, byteValue, QImage.Format_RGB888) self.m_image = QImage(Black, width, height, byteValue, QImage.Format_RGB888) self.r_image = QImage(Black, width, height, byteValue, QImage.Format_RGB888) self.c_image = QImage(Black, width, height, byteValue, QImage.Format_RGB888) self.d_label = QLabel(self._widget.graphicsView_1) d_image = QPixmap.fromImage(self.d_image) self.d_label.setPixmap(d_image) self.m_label = QLabel(self._widget.graphicsView_2) m_image = QPixmap.fromImage(self.m_image) self.m_label.setPixmap(m_image) self.r_label = QLabel(self._widget.graphicsView_3) r_image = QPixmap.fromImage(self.r_image) self.r_label.setPixmap(r_image) self.c_label = QLabel(self._widget.graphicsView_4) c_image = QPixmap.fromImage(self.c_image) self.c_label.setPixmap(c_image) @QtCore.Slot(int) def decay_moved(r): if self.is_decay_auto == 0: self.decay_pub.publish(str(r)) self.decay_rate = SliderWithValue(QtCore.Qt.Horizontal, 1) self.decay_rate.slidervalue.connect(decay_moved) self.decay_rate.setMinimum(-20) self.decay_rate.setMaximum(30) self.decay_rate.setTickInterval(1) self.decay_rate.setSingleStep(1) self.decay_rate.setPageStep(1) self.decay_rate.setTickPosition(QSlider.TicksBelow) self.decay_rate.setSizePolicy(QSizePolicy.MinimumExpanding, QSizePolicy.Fixed) self.decay_rate.setValue(-10) self.decay_pub.publish(str(self.decay_rate.slidervalue)) self._widget.decay_rate.addWidget(self.decay_rate) @QtCore.Slot(int) def decay_auto(args): self.is_decay_auto = args if args == 2: self.decay_pub.publish("auto") self.decay_pub.unregister del self.decay_pub if args == 0: self.decay_pub = rospy.Publisher("/fome/decay_rate", String, queue_size=1) self._widget.decay_auto.stateChanged.connect(decay_auto) @QtCore.Slot(int) def frame_moved(r): if self.is_frame_auto == 0: self.frame_pub.publish(str(r)) self.frame_rate = SliderWithValue(QtCore.Qt.Horizontal, 1) self.frame_rate.slidervalue.connect(frame_moved) self.frame_rate.setMinimum(10) self.frame_rate.setMaximum(2000) self.frame_rate.setTickInterval(1) self.frame_rate.setSingleStep(1) self.frame_rate.setPageStep(1) self.frame_rate.setTickPosition(QSlider.TicksBelow) self.frame_rate.setSizePolicy(QSizePolicy.MinimumExpanding, QSizePolicy.Fixed) self.frame_rate.setValue(100) self._widget.frame_rate.addWidget(self.frame_rate) @QtCore.Slot(int) def frame_auto(args): self.is_frame_auto = args if args == 2: self.frame_pub.publish("auto") self.frame_pub.unregister del self.frame_pub if args == 0: self.frame_pub = rospy.Publisher("/fome/frame_rate", String, queue_size=1) self._widget.frame_auto.stateChanged.connect(frame_auto) @QtCore.Slot(int) def segsz_moved(r): if self.is_segsz_auto == 0: self.segsz_pub.publish(str(r)) self.segsz_rate = SliderWithValue(QtCore.Qt.Horizontal, 1) self.segsz_rate.slidervalue.connect(segsz_moved) self.segsz_rate.setMinimum(100) self.segsz_rate.setMaximum(10000) self.segsz_rate.setTickInterval(10) self.segsz_rate.setSingleStep(10) self.segsz_rate.setPageStep(10) self.segsz_rate.setTickPosition(QSlider.TicksBelow) self.segsz_rate.setSizePolicy(QSizePolicy.MinimumExpanding, QSizePolicy.Fixed) self.segsz_rate.setValue(4320) self._widget.segsz_rate.addWidget(self.segsz_rate) @QtCore.Slot(int) def segsz_auto(args): self.is_segsz_auto = args if args == 2: self.segsz_pub.publish("auto") self.segsz_pub.unregister del self.segsz_pub if args == 0: self.segsz_pub = rospy.Publisher("/fome/segsz_rate", String, queue_size=1) self._widget.segsz_auto.stateChanged.connect(segsz_auto) if context.serial_number() > 1: self._widget.setWindowTitle('Estimated direction from events') context.add_widget(self._widget) def fome_dir(self, data): try: cvImage = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) height, width, byteValue = cvImage.shape byteValue = byteValue * width cv2.cvtColor(cvImage, cv2.COLOR_BGR2RGB, cvImage) d_image = QImage(cvImage, width, height, byteValue, QImage.Format_RGB888) d_image = QPixmap.fromImage(d_image) d_image = d_image.scaledToHeight(300) self.d_label.setPixmap(d_image) def fome_mag(self, data): try: cvImage = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) height, width, byteValue = cvImage.shape byteValue = byteValue * width cv2.cvtColor(cvImage, cv2.COLOR_BGR2RGB, cvImage) m_image = QImage(cvImage, width, height, byteValue, QImage.Format_RGB888) m_image = QPixmap.fromImage(m_image) m_image = m_image.scaledToHeight(300) self.m_label.setPixmap(m_image) def fome_cls(self, data): try: cvImage = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) height, width, byteValue = cvImage.shape byteValue = byteValue * width cv2.cvtColor(cvImage, cv2.COLOR_BGR2RGB, cvImage) c_image = QImage(cvImage, width, height, byteValue, QImage.Format_RGB888) c_image = QPixmap.fromImage(c_image) c_image = c_image.scaledToHeight(300) self.c_label.setPixmap(c_image) def fome_raw(self, data): try: cvImage = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) height, width, byteValue = cvImage.shape byteValue = byteValue * width cv2.cvtColor(cvImage, cv2.COLOR_BGR2RGB, cvImage) r_image = QImage(cvImage, width, height, byteValue, QImage.Format_RGB888) r_image = QPixmap.fromImage(r_image) r_image = r_image.scaledToHeight(300) self.r_label.setPixmap(r_image) 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 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()