pr2_dashboard: pr2_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('pr2_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='PR2 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('pr2_dashboard', anonymous=True)
00071         try:
00072             getattr(rxtools, "initRoscpp")
00073             rxtools.initRoscpp("pr2_dashboard_cpp", anonymous=True)
00074         except AttributeError:
00075             pass
00076         
00077         self.SetTitle('PR2 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, "Runstops"), 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
00124         self._wireless_runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path, "runstop-wireless", False)
00125         self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Wireless 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("pr2_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.get_top_level_state()
00169       if (level == -1 or level == 3):
00170         if (self._diagnostics_button.set_stale()):
00171             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
00172       elif (level >= 2):
00173         if (self._diagnostics_button.set_error()):
00174             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
00175       elif (level == 1):
00176         if (self._diagnostics_button.set_warn()):
00177             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
00178       else:
00179         if (self._diagnostics_button.set_ok()):
00180             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
00181         
00182       self.update_rosout()
00183       
00184       if (rospy.get_time() - self._last_dashboard_message_time > 5.0):
00185           self._motors_button.set_stale()
00186           self._power_state_ctrl.set_stale()
00187           [ctrl.reset() for ctrl in self._breaker_ctrls]
00188           self._runstop_ctrl.set_stale()
00189           self._wireless_runstop_ctrl.set_stale()
00190           ctrls = [self._motors_button, self._power_state_ctrl, self._runstop_ctrl, self._wireless_runstop_ctrl]
00191           ctrls.extend(self._breaker_ctrls)
00192           for ctrl in ctrls:
00193               ctrl.SetToolTip(wx.ToolTip("No message received on dashboard_agg in the last 5 seconds"))
00194         
00195       if (rospy.is_shutdown()):
00196         self.Close()
00197         
00198     def on_diagnostics_clicked(self, evt):
00199       self._diagnostics_frame.Show()
00200       self._diagnostics_frame.Raise()
00201       
00202     def on_rosout_clicked(self, evt):
00203       self._rosout_frame.Show()
00204       self._rosout_frame.Raise()
00205       
00206     def on_motors_clicked(self, evt):
00207       menu = wx.Menu()
00208       menu.Bind(wx.EVT_MENU, self.on_reset_motors, menu.Append(wx.ID_ANY, "Reset"))
00209       menu.Bind(wx.EVT_MENU, self.on_halt_motors, menu.Append(wx.ID_ANY, "Halt"))
00210       self._motors_button.toggle(True)
00211       self.PopupMenu(menu)
00212       self._motors_button.toggle(False)
00213       
00214     def on_reset_motors(self, evt):
00215       # if any of the breakers is not enabled ask if they'd like to enable them
00216       if (self._dashboard_message is not None and self._dashboard_message.power_board_state_valid):
00217           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])
00218           if (not all_breakers_enabled):
00219               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):
00220                   [breaker.set_enable() for breaker in self._breaker_ctrls]
00221         
00222       reset = rospy.ServiceProxy("pr2_etherCAT/reset_motors", std_srvs.srv.Empty)
00223        
00224       try:
00225         reset()
00226       except rospy.ServiceException, e:
00227         wx.MessageBox("Failed to reset the motors: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
00228     
00229     def on_halt_motors(self, evt):
00230       halt = rospy.ServiceProxy("pr2_etherCAT/halt_motors", std_srvs.srv.Empty)
00231        
00232       try:
00233         halt()
00234       except rospy.ServiceException, e:
00235         wx.MessageBox("Failed to halt the motors: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
00236       
00237     def dashboard_callback(self, msg):
00238       wx.CallAfter(self.new_dashboard_message, msg)
00239       
00240     def new_dashboard_message(self, msg):
00241       self._dashboard_message = msg
00242       self._last_dashboard_message_time = rospy.get_time()
00243       
00244       if (msg.motors_halted_valid):
00245         if (not msg.motors_halted.data):
00246           if (self._motors_button.set_ok()):
00247               self._motors_button.SetToolTip(wx.ToolTip("Motors: Running"))
00248         else:
00249           if (self._motors_button.set_error()):
00250               self._motors_button.SetToolTip(wx.ToolTip("Motors: Halted"))
00251       else:
00252           if (self._motors_button.set_stale()):
00253               self._motors_button.SetToolTip(wx.ToolTip("Motors: Stale"))
00254           
00255       if (msg.power_state_valid):
00256         self._power_state_ctrl.set_power_state(msg.power_state)
00257       else:
00258         self._power_state_ctrl.set_stale()
00259       
00260       if (msg.power_board_state_valid):
00261         [ctrl.set_power_board_state_msg(msg.power_board_state) for ctrl in self._breaker_ctrls]
00262         
00263         if (not msg.power_board_state.run_stop):
00264           # if the wireless stop is also off, we can't tell if the runstop is pressed or not
00265           if (not msg.power_board_state.wireless_stop):
00266             if (self._runstop_ctrl.set_warn()):
00267                 self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Unknown (Wireless is Pressed)"))
00268           else:
00269             if (self._runstop_ctrl.set_error()):
00270                 self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Pressed"))
00271         else:          
00272           if (self._runstop_ctrl.set_ok()):
00273               self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: OK"))
00274           
00275         if (not msg.power_board_state.wireless_stop):
00276           if (self._wireless_runstop_ctrl.set_error()):
00277               self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Wireless Runstop: Pressed"))
00278         else:
00279           if (self._wireless_runstop_ctrl.set_ok()):
00280               self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Wireless Runstop: OK"))
00281       else:
00282         if (self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Wireless Runstop: Stale"))):
00283             self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Stale"))
00284             [ctrl.reset() for ctrl in self._breaker_ctrls]
00285             self._runstop_ctrl.set_stale()
00286             self._wireless_runstop_ctrl.set_stale()
00287           
00288     def update_rosout(self):
00289       summary_dur = 30.0
00290       if (rospy.get_time() < 30.0):
00291           summary_dur = rospy.get_time() - 1.0
00292           
00293       if (summary_dur < 0):
00294           summary_dur = 0.0
00295     
00296       summary = self._rosout_frame.get_panel().getMessageSummary(summary_dur)
00297       
00298       if (summary.fatal or summary.error):
00299         self._rosout_button.set_error()
00300       elif (summary.warn):
00301         self._rosout_button.set_warn()
00302       else:
00303         self._rosout_button.set_ok()
00304         
00305         
00306       tooltip = ""
00307       if (summary.fatal):
00308         tooltip += "\nFatal: %s"%(summary.fatal)
00309       if (summary.error):
00310         tooltip += "\nError: %s"%(summary.error)
00311       if (summary.warn):
00312         tooltip += "\nWarn: %s"%(summary.warn)
00313       if (summary.info):
00314         tooltip += "\nInfo: %s"%(summary.info)
00315       if (summary.debug):
00316         tooltip += "\nDebug: %s"%(summary.debug)
00317       
00318       if (len(tooltip) == 0):
00319         tooltip = "Rosout: no recent activity"
00320       else:
00321         tooltip = "Rosout: recent activity:" + tooltip
00322     
00323       if (tooltip != self._rosout_button.GetToolTip().GetTip()):
00324           self._rosout_button.SetToolTip(wx.ToolTip(tooltip))
00325         
00326     def load_config(self):
00327       # Load our window options
00328       (x, y) = self.GetPositionTuple()
00329       (width, height) = self.GetSizeTuple()
00330       if (self._config.HasEntry(self._CONFIG_WINDOW_X)):
00331           x = self._config.ReadInt(self._CONFIG_WINDOW_X)
00332       if (self._config.HasEntry(self._CONFIG_WINDOW_Y)):
00333           y = self._config.ReadInt(self._CONFIG_WINDOW_Y)
00334       
00335       self.SetPosition((x, y))
00336       self.SetSize((width, height))
00337         
00338     def save_config(self):
00339       config = self._config
00340       
00341       (x, y) = self.GetPositionTuple()
00342       (width, height) = self.GetSizeTuple()
00343       config.WriteInt(self._CONFIG_WINDOW_X, x)
00344       config.WriteInt(self._CONFIG_WINDOW_Y, y)
00345       
00346       config.Flush()
00347         
00348     def on_close(self, event):
00349       self.save_config()
00350       
00351       self.Destroy()
00352
pr2_frame.py
