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