00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 import os
00032 import rospy
00033 import rospkg
00034 import time
00035
00036 from qt_gui.plugin import Plugin
00037 from python_qt_binding import loadUi
00038 from python_qt_binding.QtWidgets import QWidget, QGraphicsScene, QLabel, QGraphicsEllipseItem, QGraphicsLineItem, QGraphicsTextItem
00039 from python_qt_binding.QtCore import Qt, Signal, QTimer, QDateTime, QDate, QTime, QPoint, QEvent, QObject, pyqtSignal
00040 from python_qt_binding.QtGui import QPixmap, QFont, QPen, QColor, QBrush, QImage
00041
00042 from cmath import *
00043 from math import *
00044 from rospy.rostime import Time
00045 from numpy import isfinite
00046
00047 from dataspeed_pds_msgs.msg import Status, Channel, Relay, Mode, Script
00048
00049
00050
00051 def clickable(widget):
00052 class Filter(QObject):
00053 clicked = pyqtSignal()
00054 def eventFilter(self, obj, event):
00055
00056 if obj == widget:
00057 if event.type() == QEvent.MouseButtonRelease:
00058 if obj.rect().contains(event.pos()):
00059 self.clicked.emit()
00060
00061 return True
00062
00063 return False
00064
00065 filter = Filter(widget)
00066 widget.installEventFilter(filter)
00067 return filter.clicked
00068
00069 class PowerDistributionGui(Plugin):
00070 gui_update_timer = QTimer()
00071
00072
00073 current_status = None
00074
00075 current_amps = None
00076
00077
00078 is_currently_tab = False
00079 widget_count = 0
00080 current_tab_idx = -1
00081 raw_data_tab_idx = 5
00082
00083 def __init__(self, context):
00084 super(PowerDistributionGui, self).__init__(context)
00085
00086
00087 self.context_ = context
00088 self.init_gui('full')
00089
00090
00091 self.subscribe_topics()
00092 self.advertise_topics()
00093 self.last_update = 0
00094
00095 def init_gui(self, gui_type):
00096 if gui_type == 'full':
00097 self.spawn_full_gui()
00098
00099 self.bind_callbacks()
00100 self.reset_gui_timer()
00101
00102 def update_channel_buttons(self):
00103 for index in range(0, 24):
00104 channelStatusName = "NONE"
00105 buttonStyle = ""
00106
00107 if self.current_status.chan[index].status == Channel.ON:
00108 buttonStyle += 'background-color: rgb(78, 154, 6);'
00109 channelStatusName = "ON"
00110 elif self.current_status.chan[index].status == Channel.NO_LOAD:
00111 buttonStyle += 'background-color: rgb(78, 154, 6);'
00112 channelStatusName = "NO LOAD"
00113 elif self.current_status.chan[index].status == Channel.OVER_CURRENT:
00114 buttonStyle += 'background-color: rgb(252, 175, 62);'
00115 channelStatusName = "OVER CURRENT"
00116 elif self.current_status.chan[index].status == Channel.OFF:
00117 buttonStyle += 'background-color: rgb(130, 130, 130);'
00118 channelStatusName = "OFF"
00119 elif self.current_status.chan[index].status == Channel.BAD_RELAY:
00120 buttonStyle += 'background-color: rgb(229, 90, 90);'
00121 channelStatusName = "BAD RELAY"
00122 elif self.current_status.chan[index].status == Channel.BAD_FUSE:
00123 buttonStyle += 'background-color: rgb(229, 90, 90);'
00124 channelStatusName = "BAD FUSE"
00125
00126 buttonText = '<html><head/><body><p align="center"><span style=" font-size:12pt;">'
00127 buttonText += '#{} {}'.format(index + 1, channelStatusName)
00128 buttonText += '</span></p><p align="center"><span style=" font-size:28pt;">'
00129 buttonText += '{0:.2f} A'.format(self.current_status.chan[index].current)
00130 buttonText += '</span></p><p align="center"><span style=" font-size:8pt;">'
00131 buttonText += '</span></p></body></html>'
00132 getattr(self._widget, "buttonChannel{:0>2d}".format(index + 1)).setText(buttonText)
00133
00134 buttonStyle += 'color: rgb(0, 0, 0);'
00135 buttonStyle += 'border-radius: 12px;'
00136 buttonStyle += 'padding: 12px;'
00137 getattr(self._widget, "buttonChannel{:0>2d}".format(index + 1)).setStyleSheet(buttonStyle)
00138
00139 def update_inverter_buttons(self):
00140
00141 buttonTextMaster = '<html><head/><body><p align="center"><span style=" font-size:20pt;">Toggle Master</span></p><p align="center"><span style=" font-size:28pt;">'
00142 buttonTextMaster += 'ON' if self.current_status.master.inverter.status else 'OFF'
00143 buttonTextMaster += '</span></p></body></html>'
00144 self._widget.buttonInverterMaster.setText(buttonTextMaster)
00145
00146
00147 buttonStyleMaster = 'color: rgb(0, 0, 0);'
00148 buttonStyleMaster += 'border-radius: 12px;'
00149 buttonStyleMaster += 'padding: 12px;'
00150 buttonStyleMaster += 'background-color: rgb(78, 154, 6);' if self.current_status.master.inverter.status else 'background-color: rgb(229, 90, 90);'
00151 self._widget.buttonInverterMaster.setStyleSheet(buttonStyleMaster)
00152
00153
00154 buttonTextSlave = '<html><head/><body><p align="center"><span style=" font-size:20pt;">Toggle Slave</span></p><p align="center"><span style=" font-size:28pt;">'
00155 buttonTextSlave += 'ON' if self.current_status.slave[0].inverter.status else 'OFF'
00156 buttonTextSlave += '</span></p></body></html>'
00157 self._widget.buttonInverterSlave.setText(buttonTextSlave)
00158
00159
00160 buttonStyleSlave = 'color: rgb(0, 0, 0);'
00161 buttonStyleSlave += 'border-radius: 12px;'
00162 buttonStyleSlave += 'padding: 12px;'
00163 buttonStyleSlave += 'background-color: rgb(78, 154, 6);' if self.current_status.slave[0].inverter.status else 'background-color: rgb(229, 90, 90);'
00164 self._widget.buttonInverterSlave.setStyleSheet(buttonStyleSlave)
00165
00166 def update_mode(self):
00167 self._widget.radioButtonAuto.setChecked(self.current_status.mode.mode == Mode.AUTO)
00168 self._widget.radioButtonManual.setChecked(self.current_status.mode.mode == Mode.MANUAL)
00169 self._widget.radioButtonValet.setChecked(self.current_status.mode.mode == Mode.VALET)
00170
00171 def click_mode_auto(self):
00172 out_msg = Mode()
00173 out_msg.mode = Mode.AUTO
00174 self.pub_cmd_mode.publish(out_msg)
00175
00176 def click_mode_manual(self):
00177 out_msg = Mode()
00178 out_msg.mode = Mode.MANUAL
00179 self.pub_cmd_mode.publish(out_msg)
00180
00181 def click_mode_valet(self):
00182 out_msg = Mode()
00183 out_msg.mode = Mode.VALET
00184 self.pub_cmd_mode.publish(out_msg)
00185
00186 def recv_status(self, msg):
00187 self.current_status = msg
00188 self.last_update = time.time()
00189
00190 def subscribe_topics(self):
00191 sub_status = rospy.Subscriber('/pds/status', Status, self.recv_status)
00192
00193 def advertise_topics(self):
00194 self.pub_cmd_request = rospy.Publisher('/pds/relay', Relay, queue_size=1)
00195 self.pub_cmd_mode = rospy.Publisher('/pds/mode', Mode, queue_size=1)
00196 self.pub_cmd_script = rospy.Publisher('/pds/script', Script, queue_size=1)
00197
00198 def reset_gui_timer(self):
00199 self.gui_update_timer = QTimer(self._widget)
00200 self.gui_update_timer.setInterval(100)
00201 self.gui_update_timer.setSingleShot(False)
00202 self.gui_update_timer.timeout.connect(lambda: self.update_gui_cb())
00203 self.gui_update_timer.start()
00204
00205 def click_script_stop(self):
00206 out_msg = Script()
00207 out_msg.script = Script.NONE
00208 self.pub_cmd_script.publish(out_msg)
00209
00210 def click_script_startup(self):
00211 out_msg = Script()
00212 out_msg.script = Script.STARTUP
00213 self.pub_cmd_script.publish(out_msg)
00214
00215 def click_script_shutdown(self):
00216 out_msg = Script()
00217 out_msg.script = Script.SHUTDOWN
00218 self.pub_cmd_script.publish(out_msg)
00219
00220 def click_inverter_master(self):
00221 out_msg = Relay()
00222
00223 out_msg.channel = 48
00224 out_msg.request = Relay.TOGGLE
00225 self.pub_cmd_request.publish(out_msg)
00226
00227 def click_inverter_slave(self):
00228 out_msg = Relay()
00229
00230 out_msg.channel = 49
00231 out_msg.request = Relay.TOGGLE
00232 self.pub_cmd_request.publish(out_msg)
00233
00234 def update_gui_cb(self):
00235
00236 self._widget.labelNoData.move(
00237 self._widget.width()/2 - self._widget.labelNoData.width()/2,
00238 self._widget.height()/2 - self._widget.labelNoData.height()/2)
00239
00240 if time.time() - self.last_update > 3:
00241
00242 self.current_status = None
00243 if self.current_status is None:
00244 self._widget.labelNoData.show()
00245 return
00246 else:
00247 self._widget.labelNoData.hide()
00248 self.update_channel_buttons()
00249 self.update_inverter_buttons()
00250 self.update_mode()
00251
00252 def get_size(self, widget):
00253 print(str(widget.width()) + "," + str(widget.height()))
00254
00255 def spawn_full_gui(self):
00256 super(PowerDistributionGui, self).__init__(self.context_)
00257
00258 self.setObjectName('PowerDistributionGui')
00259
00260
00261 self._widget = QWidget()
00262
00263
00264 ui_file = os.path.join(rospkg.RosPack().get_path('dataspeed_pds_rqt'), 'resource', 'PowerDistributionGui.ui')
00265
00266 loadUi(ui_file, self._widget)
00267
00268 self._widget.setObjectName('PowerDistributionGui' + str(self.widget_count))
00269 self.widget_count += 1
00270
00271 self.context_.add_widget(self._widget)
00272
00273 def spawn_tab_gui(self):
00274 super(PowerDistributionGui, self).__init__(self.context_)
00275
00276 self.setObjectName('PowerDistributionGui')
00277
00278
00279 self._widget = QWidget()
00280
00281
00282 ui_file = os.path.join(rospkg.RosPack().get_path('dataspeed_pds_rqt'), 'resource', 'PowerDistributionGuiTabs.ui')
00283
00284 loadUi(ui_file, self._widget)
00285
00286 self._widget.setObjectName('PowerDistributionGui' + str(self.widget_count))
00287 self.widget_count += 1
00288
00289 self.context_.add_widget(self._widget)
00290
00291 def create_click_channel(self, number):
00292 def click_channel():
00293 out_msg = Relay()
00294
00295 out_msg.channel = number - 1
00296 out_msg.request = Relay.TOGGLE
00297 self.pub_cmd_request.publish(out_msg)
00298 return click_channel
00299
00300 def bind_callbacks(self):
00301 clickable(self._widget.buttonInverterMaster).connect(self.click_inverter_master)
00302 clickable(self._widget.buttonInverterSlave).connect(self.click_inverter_slave)
00303 self._widget.radioButtonAuto.clicked.connect(self.click_mode_auto)
00304 self._widget.radioButtonManual.clicked.connect(self.click_mode_manual)
00305 self._widget.radioButtonValet.clicked.connect(self.click_mode_valet)
00306 self._widget.buttonScriptStop.clicked.connect(self.click_script_stop)
00307 self._widget.buttonScriptStartup.clicked.connect(self.click_script_startup)
00308 self._widget.buttonScriptShutdown.clicked.connect(self.click_script_shutdown)
00309
00310 for index in range(1, 24 + 1):
00311 clickable(getattr(self._widget, "buttonChannel{:0>2d}".format(index))).connect(self.create_click_channel(index))
00312
00313 @staticmethod
00314 def add_arguments(parser):
00315 group = parser.add_argument_group('Options for dataspeed_pds_rqt plugin')
00316
00317
00318 def shutdown_plugin(self):
00319 pass