cob_dashboard: cob_dashboard: pr2

00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2008, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of the Willow Garage nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
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         # Diagnostics
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         # Rosout
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         # Motors
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         # Breakers
00107         breaker_names = ["Left Arm", "Base/Body", "Right Arm"]
00108         self._breaker_ctrls = []
00109 #        for i in xrange(0, 3):
00110 #          ctrl = BreakerControl(self, wx.ID_ANY, i, breaker_names[i], icons_path)
00111 #          ctrl.SetToolTip(wx.ToolTip("Breaker %s"%(breaker_names[i])))
00112 #          self._breaker_ctrls.append(ctrl)
00113 #          static_sizer.Add(ctrl, 0)
00114         
00115         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "EM"), wx.HORIZONTAL)
00116         sizer.Add(static_sizer, 0)
00117         
00118         # run-stop
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         # Wireless run-stop ## for cob this is misused as Laser Stop
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         # Battery State
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 #      level = self._diagnostics_frame._diagnostics_panel.get_top_level_state()
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       # if any of the breakers is not enabled ask if they'd like to enable them
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           # if the wireless stop is also off, we can't tell if the runstop is pressed or not
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       # Load our window options
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
pr2_frame.py
