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('cob_dashboard')
00035
00036 import wx
00037 import wx.aui
00038 import wx.py.shell
00039 import rxtools
00040 import rxtools.cppwidgets as rxtools
00041
00042 import robot_monitor
00043 from robot_monitor.robot_monitor_panel import RobotMonitorPanel
00044 from pr2_msgs.msg import PowerState, PowerBoardState, DashboardState
00045 from pr2_power_board.srv import PowerBoardCommand, PowerBoardCommandRequest
00046 import std_msgs.msg
00047 import std_srvs.srv
00048
00049 import rospy
00050 from roslib import rosenv
00051
00052 from os import path
00053 import threading
00054
00055 from status_control import StatusControl
00056 from breaker_control import BreakerControl
00057 from power_state_control import PowerStateControl
00058 from diagnostics_frame import DiagnosticsFrame
00059 from rosout_frame import RosoutFrame
00060
00061 class PR2Frame(wx.Frame):
00062 _CONFIG_WINDOW_X="/Window/X"
00063 _CONFIG_WINDOW_Y="/Window/Y"
00064
00065 def __init__(self, parent, id=wx.ID_ANY, title='cob dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX|wx.STAY_ON_TOP):
00066 wx.Frame.__init__(self, parent, id, title, pos, size, style)
00067
00068 wx.InitAllImageHandlers()
00069
00070 rospy.init_node('cob_dashboard', anonymous=True)
00071 try:
00072 getattr(rxtools, "initRoscpp")
00073 rxtools.initRoscpp("cob_dashboard_cpp", anonymous=True)
00074 except AttributeError:
00075 pass
00076
00077 self.SetTitle('cob dashboard (%s)'%rosenv.get_master_uri())
00078
00079 icons_path = path.join(roslib.packages.get_pkg_dir('pr2_dashboard'), "icons/")
00080
00081 sizer = wx.BoxSizer(wx.HORIZONTAL)
00082 self.SetSizer(sizer)
00083
00084 static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagnostic"), wx.HORIZONTAL)
00085 sizer.Add(static_sizer, 0)
00086
00087
00088 self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "diag", True)
00089 self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
00090 static_sizer.Add(self._diagnostics_button, 0)
00091
00092
00093 self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "rosout", True)
00094 self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
00095 static_sizer.Add(self._rosout_button, 0)
00096
00097
00098 self._motors_button = StatusControl(self, wx.ID_ANY, icons_path, "motor", True)
00099 self._motors_button.SetToolTip(wx.ToolTip("Motors"))
00100 static_sizer.Add(self._motors_button, 0)
00101 self._motors_button.Bind(wx.EVT_LEFT_DOWN, self.on_motors_clicked)
00102
00103 static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Breakers"), wx.HORIZONTAL)
00104 sizer.Add(static_sizer, 0)
00105
00106
00107 breaker_names = ["Left Arm", "Base/Body", "Right Arm"]
00108 self._breaker_ctrls = []
00109
00110
00111
00112
00113
00114
00115 static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "EM"), wx.HORIZONTAL)
00116 sizer.Add(static_sizer, 0)
00117
00118
00119 self._runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path, "runstop", False)
00120 self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Unknown"))
00121 static_sizer.Add(self._runstop_ctrl, 0)
00122
00123
00124 self._wireless_runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path, "runstop-wireless", False)
00125 self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Laser Runstop: Unknown"))
00126 static_sizer.Add(self._wireless_runstop_ctrl, 0)
00127
00128 static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Battery"), wx.HORIZONTAL)
00129 sizer.Add(static_sizer, 0)
00130
00131
00132 self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
00133 self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
00134 static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
00135
00136 self._config = wx.Config("cob_dashboard")
00137
00138 self.Bind(wx.EVT_CLOSE, self.on_close)
00139
00140 self.Layout()
00141 self.Fit()
00142
00143 self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
00144 self._diagnostics_frame.Hide()
00145 self._diagnostics_frame.Center()
00146 self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
00147
00148 self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
00149 self._rosout_frame.Hide()
00150 self._rosout_frame.Center()
00151 self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
00152
00153 self.load_config()
00154
00155 self._timer = wx.Timer(self)
00156 self.Bind(wx.EVT_TIMER, self.on_timer)
00157 self._timer.Start(500)
00158
00159 self._dashboard_agg_sub = rospy.Subscriber('dashboard_agg', DashboardState, self.dashboard_callback)
00160
00161 self._dashboard_message = None
00162 self._last_dashboard_message_time = 0.0
00163
00164 def __del__(self):
00165 self._dashboard_agg_sub.unregister()
00166
00167 def on_timer(self, evt):
00168
00169 level = self._diagnostics_frame.get_top_level_state()
00170 if (level == -1 or level == 3):
00171 if (self._diagnostics_button.set_stale()):
00172 self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
00173 elif (level >= 2):
00174 if (self._diagnostics_button.set_error()):
00175 self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
00176 elif (level == 1):
00177 if (self._diagnostics_button.set_warn()):
00178 self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
00179 else:
00180 if (self._diagnostics_button.set_ok()):
00181 self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
00182
00183 self.update_rosout()
00184
00185 if (rospy.get_time() - self._last_dashboard_message_time > 5.0):
00186 self._motors_button.set_stale()
00187 self._power_state_ctrl.set_stale()
00188 [ctrl.reset() for ctrl in self._breaker_ctrls]
00189 self._runstop_ctrl.set_stale()
00190 self._wireless_runstop_ctrl.set_stale()
00191 ctrls = [self._motors_button, self._power_state_ctrl, self._runstop_ctrl, self._wireless_runstop_ctrl]
00192 ctrls.extend(self._breaker_ctrls)
00193 for ctrl in ctrls:
00194 ctrl.SetToolTip(wx.ToolTip("No message received on dashboard_agg in the last 5 seconds"))
00195
00196 if (rospy.is_shutdown()):
00197 self.Close()
00198
00199 def on_diagnostics_clicked(self, evt):
00200 self._diagnostics_frame.Show()
00201 self._diagnostics_frame.Raise()
00202
00203 def on_rosout_clicked(self, evt):
00204 self._rosout_frame.Show()
00205 self._rosout_frame.Raise()
00206
00207 def on_motors_clicked(self, evt):
00208 menu = wx.Menu()
00209 menu.Bind(wx.EVT_MENU, self.on_reset_motors, menu.Append(wx.ID_ANY, "Reset"))
00210 menu.Bind(wx.EVT_MENU, self.on_halt_motors, menu.Append(wx.ID_ANY, "Halt"))
00211 self._motors_button.toggle(True)
00212 self.PopupMenu(menu)
00213 self._motors_button.toggle(False)
00214
00215 def on_reset_motors(self, evt):
00216
00217 if (self._dashboard_message is not None and self._dashboard_message.power_board_state_valid):
00218 all_breakers_enabled = reduce(lambda x,y: x and y, [state == PowerBoardState.STATE_ON for state in self._dashboard_message.power_board_state.circuit_state])
00219 if (not all_breakers_enabled):
00220 if (wx.MessageBox("Resetting the motors may not work because not all breakers are enabled. Enable all the breakers first?", "Enable Breakers?", wx.YES_NO|wx.ICON_QUESTION, self) == wx.YES):
00221 [breaker.set_enable() for breaker in self._breaker_ctrls]
00222
00223 reset = rospy.ServiceProxy("pr2_etherCAT/reset_motors", std_srvs.srv.Empty)
00224
00225 try:
00226 reset()
00227 except rospy.ServiceException, e:
00228 wx.MessageBox("Failed to reset the motors: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
00229
00230 def on_halt_motors(self, evt):
00231 halt = rospy.ServiceProxy("pr2_etherCAT/halt_motors", std_srvs.srv.Empty)
00232
00233 try:
00234 halt()
00235 except rospy.ServiceException, e:
00236 wx.MessageBox("Failed to halt the motors: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
00237
00238 def dashboard_callback(self, msg):
00239 wx.CallAfter(self.new_dashboard_message, msg)
00240
00241 def new_dashboard_message(self, msg):
00242 self._dashboard_message = msg
00243 self._last_dashboard_message_time = rospy.get_time()
00244
00245 if (msg.motors_halted_valid):
00246 if (not msg.motors_halted.data):
00247 if (self._motors_button.set_ok()):
00248 self._motors_button.SetToolTip(wx.ToolTip("Motors: Running"))
00249 else:
00250 if (self._motors_button.set_error()):
00251 self._motors_button.SetToolTip(wx.ToolTip("Motors: Halted"))
00252 else:
00253 if (self._motors_button.set_stale()):
00254 self._motors_button.SetToolTip(wx.ToolTip("Motors: Stale"))
00255
00256 if (msg.power_state_valid):
00257 self._power_state_ctrl.set_power_state(msg.power_state)
00258 else:
00259 self._power_state_ctrl.set_stale()
00260
00261 if (msg.power_board_state_valid):
00262 [ctrl.set_power_board_state_msg(msg.power_board_state) for ctrl in self._breaker_ctrls]
00263
00264 if (not msg.power_board_state.run_stop):
00265
00266 if (not msg.power_board_state.wireless_stop):
00267 if (self._runstop_ctrl.set_warn()):
00268 self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Unknown (Laser Stop is activated)"))
00269 else:
00270 if (self._runstop_ctrl.set_error()):
00271 self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Pressed"))
00272 else:
00273 if (self._runstop_ctrl.set_ok()):
00274 self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: OK"))
00275
00276 if (not msg.power_board_state.wireless_stop):
00277 if (self._wireless_runstop_ctrl.set_error()):
00278 self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Laser Stop: activated"))
00279 else:
00280 if (self._wireless_runstop_ctrl.set_ok()):
00281 self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Laser Stop: OK"))
00282 else:
00283 if (self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Laser Stop: Stale"))):
00284 self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Stale"))
00285 [ctrl.reset() for ctrl in self._breaker_ctrls]
00286 self._runstop_ctrl.set_stale()
00287 self._wireless_runstop_ctrl.set_stale()
00288
00289 def update_rosout(self):
00290 summary_dur = 30.0
00291 if (rospy.get_time() < 30.0):
00292 summary_dur = rospy.get_time() - 1.0
00293
00294 if (summary_dur < 0):
00295 summary_dur = 0.0
00296
00297 summary = self._rosout_frame.get_panel().getMessageSummary(summary_dur)
00298
00299 if (summary.fatal or summary.error):
00300 self._rosout_button.set_error()
00301 elif (summary.warn):
00302 self._rosout_button.set_warn()
00303 else:
00304 self._rosout_button.set_ok()
00305
00306
00307 tooltip = ""
00308 if (summary.fatal):
00309 tooltip += "\nFatal: %s"%(summary.fatal)
00310 if (summary.error):
00311 tooltip += "\nError: %s"%(summary.error)
00312 if (summary.warn):
00313 tooltip += "\nWarn: %s"%(summary.warn)
00314 if (summary.info):
00315 tooltip += "\nInfo: %s"%(summary.info)
00316 if (summary.debug):
00317 tooltip += "\nDebug: %s"%(summary.debug)
00318
00319 if (len(tooltip) == 0):
00320 tooltip = "Rosout: no recent activity"
00321 else:
00322 tooltip = "Rosout: recent activity:" + tooltip
00323
00324 if (tooltip != self._rosout_button.GetToolTip().GetTip()):
00325 self._rosout_button.SetToolTip(wx.ToolTip(tooltip))
00326
00327 def load_config(self):
00328
00329 (x, y) = self.GetPositionTuple()
00330 (width, height) = self.GetSizeTuple()
00331 if (self._config.HasEntry(self._CONFIG_WINDOW_X)):
00332 x = self._config.ReadInt(self._CONFIG_WINDOW_X)
00333 if (self._config.HasEntry(self._CONFIG_WINDOW_Y)):
00334 y = self._config.ReadInt(self._CONFIG_WINDOW_Y)
00335
00336 self.SetPosition((x, y))
00337 self.SetSize((width, height))
00338
00339 def save_config(self):
00340 config = self._config
00341
00342 (x, y) = self.GetPositionTuple()
00343 (width, height) = self.GetSizeTuple()
00344 config.WriteInt(self._CONFIG_WINDOW_X, x)
00345 config.WriteInt(self._CONFIG_WINDOW_Y, y)
00346
00347 config.Flush()
00348
00349 def on_close(self, event):
00350 self.save_config()
00351
00352 self.Destroy()
00353