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
00032
00033
00034
00035
00036 PKG = 'pr2_power_board'
00037
00038 import roslib
00039 roslib.load_manifest(PKG)
00040
00041 import sys
00042 import re
00043 import rospy
00044 from diagnostic_msgs.msg import *
00045 from pr2_power_board.srv import *
00046
00047 WXVER = '2.8'
00048 import wxversion
00049 if wxversion.checkInstalled(WXVER):
00050 wxversion.select(WXVER)
00051 else:
00052 print >> sys.stderr, 'This application requires wxPython version %s' % (WXVER)
00053 sys.exit(1)
00054
00055 import wx
00056 from wx import xrc
00057
00058 import threading, time
00059
00060 class PowerBoardPanel(wx.Panel):
00061 def __init__(self, parent):
00062 wx.Panel.__init__(self, parent, wx.ID_ANY)
00063
00064 self._mutex = threading.Lock()
00065
00066 xrc_path = roslib.packages.get_pkg_dir(PKG) + '/ui/pr2_power_board_panel.xrc'
00067
00068 self._xrc = xrc.XmlResource(xrc_path)
00069 self._real_panel = self._xrc.LoadPanel(self, 'PowerBoardPanel')
00070
00071 serialBoxSizer = wx.BoxSizer(wx.HORIZONTAL)
00072 self.myList = wx.ComboBox(self, -1, size=(150,30), style=wx.CB_READONLY)
00073 self.Bind(wx.EVT_COMBOBOX, lambda e: self.chooseBoard(self.myList.GetValue()))
00074 serialBoxSizer.Add( wx.StaticText( self, -1, "Board ") )
00075 serialBoxSizer.Add( self.myList )
00076 serialBoxSizer.Add( wx.StaticText( self, -1, "Serial ") )
00077 self.serialText = wx.TextCtrl( self, -1, size=(150,30))
00078 self.serialText.SetEditable(False)
00079 self.boardList = dict();
00080 serialBoxSizer.Add( self.serialText )
00081
00082 sizer = wx.BoxSizer(wx.VERTICAL)
00083 sizer.Add( serialBoxSizer, 0, wx.TOP)
00084 sizer.Add(self._real_panel, 0, wx.BOTTOM)
00085 self.SetSizer(sizer)
00086
00087 self._real_panel.Bind(wx.EVT_BUTTON, self.EnableCB0, id=xrc.XRCID('m_button1'))
00088 self._real_panel.Bind(wx.EVT_BUTTON, self.EnableCB1, id=xrc.XRCID('m_button11'))
00089 self._real_panel.Bind(wx.EVT_BUTTON, self.EnableCB2, id=xrc.XRCID('m_button12'))
00090
00091 self._real_panel.Bind(wx.EVT_BUTTON, self.StandbyCB0, id=xrc.XRCID('m_button2'))
00092 self._real_panel.Bind(wx.EVT_BUTTON, self.StandbyCB1, id=xrc.XRCID('m_button21'))
00093 self._real_panel.Bind(wx.EVT_BUTTON, self.StandbyCB2, id=xrc.XRCID('m_button22'))
00094
00095 self._real_panel.Bind(wx.EVT_BUTTON, self.ResetCB0, id=xrc.XRCID('m_button3'))
00096 self._real_panel.Bind(wx.EVT_BUTTON, self.ResetCB1, id=xrc.XRCID('m_button31'))
00097 self._real_panel.Bind(wx.EVT_BUTTON, self.ResetCB2, id=xrc.XRCID('m_button32'))
00098
00099 self._real_panel.Bind(wx.EVT_BUTTON, self.DisableCB0, id=xrc.XRCID('cb0_disable'))
00100 self._real_panel.Bind(wx.EVT_BUTTON, self.DisableCB1, id=xrc.XRCID('cb1_disable'))
00101 self._real_panel.Bind(wx.EVT_BUTTON, self.DisableCB2, id=xrc.XRCID('cb2_disable'))
00102
00103 self._real_panel.Bind(wx.EVT_BUTTON, self.ResetCurrent, id=xrc.XRCID('button_reset_current'))
00104 self._real_panel.Bind(wx.EVT_BUTTON, self.ResetTransitions, id=xrc.XRCID('button_reset_transitions'))
00105 rospy.Subscriber("/diagnostics", DiagnosticArray, self.diagnostics_callback)
00106
00107 self.power_control = rospy.ServiceProxy('power_board/control', PowerBoardCommand)
00108
00109
00110 self.voltages = [0,0,0]
00111 self.breaker_state = ["", "", ""]
00112 self.estop_wireless_status = "uninitialized, no power_node sending data"
00113 self.estop_button_status = "uninitialized"
00114
00115 self.breaker0_status = xrc.XRCCTRL(self._real_panel, 'm_textCtrl1')
00116 self.breaker1_status = xrc.XRCCTRL(self._real_panel, 'm_textCtrl11')
00117 self.breaker2_status = xrc.XRCCTRL(self._real_panel, 'm_textCtrl12')
00118
00119
00120 self.estop_status = xrc.XRCCTRL(self._real_panel, 'm_textCtrl9')
00121
00122 self.breaker0_status.SetEditable(False)
00123 self.breaker1_status.SetEditable(False)
00124 self.breaker2_status.SetEditable(False)
00125 self.estop_status.SetEditable(False)
00126
00127 self.breaker0_status.SetEditable(False)
00128 self.breaker1_status.SetEditable(False)
00129 self.breaker2_status.SetEditable(False)
00130 self.estop_status.SetEditable(False)
00131
00132 self._messages = []
00133
00134 self.myList.SetValue("none")
00135 self.currentBoard = "none"
00136
00137 def chooseBoard(self, board):
00138
00139 self.currentBoard = board
00140 self.serialText.Clear()
00141 self.serialText.WriteText( str(self.boardList[self.currentBoard]) )
00142
00143 def addBoard( self, status ):
00144 name = str(status.name)
00145 serial = int(0)
00146 for strvals in status.values:
00147 if (strvals.key == "Serial Number"):
00148 serial = int(strvals.value)
00149 rospy.logerr("Adding: %s serial=%d" %(name,serial))
00150 self.myList.Append(name);
00151 self.boardList[name] = serial
00152
00153 if self.myList.Count == 1:
00154 self.myList.SetSelection(0)
00155 self.chooseBoard(name)
00156
00157 def diagnostics_callback(self, message):
00158 try:
00159 self._mutex.acquire()
00160 except:
00161 return
00162 self._messages.append(message)
00163 self._mutex.release()
00164
00165 wx.CallAfter(self.new_message)
00166
00167 def new_message(self):
00168
00169
00170 self._mutex.acquire()
00171
00172
00173 for message in self._messages:
00174
00175 for status in message.status:
00176 if( (status.name.startswith("Power board") & (self.myList.FindString( status.name ) == wx.NOT_FOUND)) ):
00177 self.addBoard( status )
00178
00179 if (status.name == self.currentBoard):
00180
00181 if( status.level == 0 ):
00182 self._real_panel.SetBackgroundColour("LIGHT_GREY")
00183 self._real_panel.Enable(True)
00184 elif (status.level == 0):
00185 self._real_panel.SetBackgroundColour("Orange")
00186 else:
00187 self._real_panel.SetBackgroundColour("RED")
00188
00189
00190 for value in status.values:
00191 if (value.key == "Breaker 0 Voltage"):
00192 self.voltages[0] = value.value
00193 if (value.key == "Breaker 1 Voltage"):
00194 self.voltages[1] = value.value
00195 if (value.key == "Breaker 2 Voltage"):
00196 self.voltages[2] = value.value
00197 if (value.key == "RunStop Button Status"):
00198 self.estop_wireless_status = value.value
00199 if (value.key == "RunStop Status"):
00200 self.estop_button_status = value.value
00201
00202 for strvals in status.values:
00203 if (re.match('Breaker 0',strvals.key,re.IGNORECASE)):
00204 self.breaker_state[0] = strvals.value
00205 if (re.match('Breaker 1',strvals.key,re.IGNORECASE)):
00206 self.breaker_state[1] = strvals.value
00207 if (re.match('Breaker 2',strvals.key,re.IGNORECASE)):
00208 self.breaker_state[2] = strvals.value
00209
00210
00211
00212
00213
00214
00215 self.breaker0_status.SetValue("%s @ %sV"%(self.breaker_state[0], self.voltages[0]))
00216 self.breaker1_status.SetValue("%s @ %sV"%(self.breaker_state[1], self.voltages[1]))
00217 self.breaker2_status.SetValue("%s @ %sV"%(self.breaker_state[2], self.voltages[2]))
00218 if self.breaker_state[0] == "Standby":
00219 self.breaker0_status.SetBackgroundColour("Orange")
00220 elif self.breaker_state[0] in ("On", "Enabled"):
00221 self.breaker0_status.SetBackgroundColour("Light Green")
00222 else:
00223 self.breaker0_status.SetBackgroundColour("Red")
00224
00225 if self.breaker_state[1] == "Standby":
00226 self.breaker1_status.SetBackgroundColour("Orange")
00227 elif self.breaker_state[1] in ("On", "Enabled"):
00228 self.breaker1_status.SetBackgroundColour("Light Green")
00229 else:
00230 self.breaker1_status.SetBackgroundColour("Red")
00231
00232 if self.breaker_state[2] == "Standby":
00233 self.breaker2_status.SetBackgroundColour("Orange")
00234 elif self.breaker_state[2] in ("On", "Enabled"):
00235 self.breaker2_status.SetBackgroundColour("Light Green")
00236 else:
00237 self.breaker2_status.SetBackgroundColour("Red")
00238
00239 if self.estop_button_status == "False" or self.estop_wireless_status == "False":
00240 estop_status_temp = "Stop"
00241 self.estop_status.SetBackgroundColour("Red")
00242 else:
00243 estop_status_temp = "Run"
00244 self.estop_status.SetBackgroundColour("Light Green")
00245
00246 self.estop_status.SetValue("RunStop Status: %s Button(%s) Wireless(%s)"%(estop_status_temp, self.estop_button_status, self.estop_wireless_status))
00247
00248
00249 self._messages = []
00250
00251 self._mutex.release()
00252
00253 self.Refresh()
00254
00255
00256
00257 def EnableCB0(self, event):
00258 try:
00259 self.power_control( self.boardList[self.currentBoard], 0, "start", 0)
00260 except rospy.ServiceException, e:
00261 rospy.logerr("Service Call Failed: %s"%e)
00262
00263 def EnableCB1(self, event):
00264 try:
00265 self.power_control( self.boardList[self.currentBoard], 1, "start", 0)
00266 except rospy.ServiceException, e:
00267 rospy.logerr("Service Call Failed: %s"%e)
00268
00269 def EnableCB2(self, event):
00270 try:
00271 self.power_control( self.boardList[self.currentBoard], 2, "start", 0)
00272 except rospy.ServiceException, e:
00273 rospy.logerr("Service Call Failed: %s"%e)
00274
00275
00276 def StandbyCB0(self, event):
00277 try:
00278 self.power_control( self.boardList[self.currentBoard], 0, "stop", 0)
00279 except rospy.ServiceException, e:
00280 rospy.logerr("Service Call Failed: %s"%e)
00281
00282 def StandbyCB1(self, event):
00283 try:
00284 self.power_control( self.boardList[self.currentBoard], 1, "stop", 0)
00285 except rospy.ServiceException, e:
00286 rospy.logerr("Service Call Failed: %s"%e)
00287
00288 def StandbyCB2(self, event):
00289 try:
00290 self.power_control( self.boardList[self.currentBoard], 2, "stop", 0)
00291 except rospy.ServiceException, e:
00292 rospy.logerr("Service Call Failed: %s"%e)
00293
00294
00295 def ResetCB0(self, event):
00296 try:
00297 self.power_control( self.boardList[self.currentBoard], 0, "reset", 0)
00298 except rospy.ServiceException, e:
00299 rospy.logerr("Service Call Failed: %s"%e)
00300
00301 def ResetCB1(self, event):
00302 try:
00303 self.power_control( self.boardList[self.currentBoard], 1, "reset", 0)
00304 except rospy.ServiceException, e:
00305 rospy.logerr("Service Call Failed: %s"%e)
00306
00307 def ResetCB2(self, event):
00308 try:
00309 self.power_control( self.boardList[self.currentBoard], 2, "reset", 0)
00310 except rospy.ServiceException, e:
00311 rospy.logerr("Service Call Failed: %s"%e)
00312
00313
00314 def DisableCB0(self, event):
00315 try:
00316 self.power_control( self.boardList[self.currentBoard], 0, "disable", 0)
00317 except rospy.ServiceException, e:
00318 rospy.logerr("Service Call Failed: %s"%e)
00319
00320 def DisableCB1(self, event):
00321 try:
00322 self.power_control( self.boardList[self.currentBoard], 1, "disable", 0)
00323 except rospy.ServiceException, e:
00324 rospy.logerr("Service Call Failed: %s"%e)
00325
00326 def DisableCB2(self, event):
00327 try:
00328 self.power_control( self.boardList[self.currentBoard], 2, "disable", 0)
00329 except rospy.ServiceException, e:
00330 rospy.logerr("Service Call Failed: %s"%e)
00331
00332
00333 def ResetCurrent(self, event):
00334 try:
00335 self.power_control( self.boardList[self.currentBoard], 0, "none", 1)
00336 except rospy.ServiceException, e:
00337 rospy.logerr("Service Call Failed: %s"%e)
00338 def ResetTransitions(self, event):
00339 try:
00340 self.power_control( self.boardList[self.currentBoard], 0, "none", 2)
00341 except rospy.ServiceException, e:
00342 rospy.logerr("Service Call Failed: %s"%e)
00343
00344