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