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 import roslib
00034 roslib.load_manifest('pr2_dashboard')
00035
00036 import wx
00037 import rospy
00038
00039 from pr2_msgs.msg import PowerState, PowerBoardState
00040 from pr2_power_board.srv import PowerBoardCommand, PowerBoardCommandRequest
00041
00042 from status_control import StatusControl
00043
00044 from os import path
00045
00046 breaker_prefixes = ("breakerleft", "breakerbase", "breakerright")
00047
00048 class BreakerControl(StatusControl):
00049 def __init__(self, parent, id, breaker_index, breaker_name, icons_path):
00050 StatusControl.__init__(self, parent, id, icons_path, breaker_prefixes[breaker_index], True)
00051
00052 self.Bind(wx.EVT_LEFT_DOWN, self.on_left_down)
00053
00054 self._power_control = rospy.ServiceProxy('power_board/control', PowerBoardCommand)
00055 self._serial = 0
00056 self._index = breaker_index
00057 self._name = breaker_name
00058 self._power_board_state = None
00059 self._last_status_msg = None
00060
00061 def on_left_down(self, evt):
00062 menu = wx.Menu()
00063 menu.Bind(wx.EVT_MENU, self.on_enable, menu.Append(wx.ID_ANY, "Enable"))
00064 menu.Bind(wx.EVT_MENU, self.on_standby, menu.Append(wx.ID_ANY, "Standby"))
00065 menu.Bind(wx.EVT_MENU, self.on_disable, menu.Append(wx.ID_ANY, "Disable"))
00066 menu.AppendSeparator()
00067 menu.Bind(wx.EVT_MENU, self.on_enable_all, menu.Append(wx.ID_ANY, "Enable All Breakers"))
00068 menu.Bind(wx.EVT_MENU, self.on_standby_all, menu.Append(wx.ID_ANY, "Standby All Breakers"))
00069 menu.Bind(wx.EVT_MENU, self.on_disable_all, menu.Append(wx.ID_ANY, "Disable All Breakers"))
00070
00071 self.toggle(True)
00072 self.PopupMenu(menu)
00073 self.toggle(False)
00074
00075 def control(self, breaker, cmd):
00076 if (not self._power_board_state):
00077 wx.MessageBox("Cannot control breakers until we have received a power board state message", "Error", wx.OK|wx.ICON_ERROR)
00078 return False
00079
00080 if (not self._power_board_state.run_stop or not self._power_board_state.wireless_stop):
00081 if (cmd == "start"):
00082 wx.MessageBox("Breakers will not enable because one of the runstops is pressed", "Error", wx.OK|wx.ICON_ERROR)
00083 return False
00084
00085 try:
00086 power_cmd = PowerBoardCommandRequest()
00087 power_cmd.breaker_number = breaker
00088 power_cmd.command = cmd
00089 power_cmd.serial_number = self._serial
00090 self._power_control(power_cmd)
00091
00092 return True
00093 except rospy.ServiceException, e:
00094 wx.MessageBox("Service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
00095 return False
00096
00097 return False
00098
00099 def control3(self, cmd):
00100 if (not self.control(0, cmd)):
00101 return False
00102 if (not self.control(1, cmd)):
00103 return False
00104 if (not self.control(2, cmd)):
00105 return False
00106
00107 return True
00108
00109 def on_enable(self, evt):
00110 self.set_enable()
00111
00112 def on_standby(self, evt):
00113 self.set_standby()
00114
00115 def on_disable(self, evt):
00116 self.set_disable()
00117
00118 def on_enable_all(self, evt):
00119 self.set_enable_all()
00120
00121 def on_standby_all(self, evt):
00122 self.set_standby_all()
00123
00124 def on_disable_all(self, evt):
00125 self.set_disable_all()
00126
00127 def set_enable(self):
00128 if (not self.control(self._index, "reset")):
00129 return
00130
00131 self.control(self._index, "start")
00132
00133 def set_standby(self):
00134 if (not self.control(self._index, "reset")):
00135 return
00136
00137 self.control(self._index, "stop")
00138
00139 def set_disable(self):
00140 self.control(self._index, "disable")
00141
00142 def set_enable_all(self):
00143 if (not self.control3("reset")):
00144 return
00145
00146 self.control3("start")
00147
00148 def set_standby_all(self):
00149 if (not self.control3("reset")):
00150 return
00151
00152 self.control3("stop")
00153
00154 def set_disable_all(self):
00155 self.control3("disable")
00156
00157 def set_power_board_state_msg(self, msg):
00158 last_voltage = msg.circuit_voltage[self._index]
00159
00160 self._power_board_state = msg
00161 self._serial = msg.serial_num
00162
00163 status_msg = "OK"
00164
00165 if (msg.circuit_state[self._index] == PowerBoardState.STATE_DISABLED):
00166 self.set_error()
00167 status_msg = "Disabled"
00168 elif (msg.circuit_state[self._index] == PowerBoardState.STATE_NOPOWER):
00169 self.set_error()
00170 status_msg = "No Power"
00171 elif (msg.circuit_state[self._index] == PowerBoardState.STATE_STANDBY):
00172 self.set_warn()
00173 status_msg = "Standby"
00174 else:
00175 self.set_ok()
00176
00177 if (status_msg != self._last_status_msg or abs(last_voltage - msg.circuit_voltage[self._index]) >= 0.1):
00178 self.SetToolTip(wx.ToolTip("Breaker %s (index=[%s], voltage=[%.02f])) State: %s"%(self._name, self._index, msg.circuit_voltage[self._index], status_msg)))
00179 self._last_status_msg = status_msg
00180
00181 def reset(self):
00182 self.set_stale()
00183 self.SetToolTip(wx.ToolTip("Breaker %s: Stale"%(self._name)))
