pr2_power_board: pr2_power_board: pr2_power_board

00001 
00002 #!/usr/bin/env python
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2008, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of the Willow Garage nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
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         #rospy.logerr("choose: %s" %self.myList.GetValue())
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             #rospy.logerr(message)
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                         #self._real_panel.Enable(False)
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                     #rospy.logerr("Voltages: %.1f %.1f %.1f"%(self.voltages[0],self.voltages[1], self.voltages[2]))
00213                     #rospy.logerr("States: %s %s %s"%(self.breaker_state[0], self.breaker_state[1], self.breaker_state[2]))
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         #rospy.logerr("Enable CB0")
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         #rospy.logerr("Enable CB1")
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         #rospy.logerr("Enable CB2")
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         #rospy.logerr("Standby CB0")
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         #rospy.logerr("Standby CB1")
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         #rospy.logerr("Standby CB2")
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         #rospy.logerr("Reset CB0")
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         #rospy.logerr("Reset CB1")
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         #rospy.logerr("Reset CB2")
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         #rospy.logerr("Disable CB0")
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         #rospy.logerr("Disable CB1")
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         #rospy.logerr("Disable CB2")
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
pr2_power_board_panel.py
