p2os_dashboard: p2os_frame.py Source File

Go to the documentation of this file.
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 #       modifications to pr2_frame.py to work with p2os_dashboard
00034 #       Copyright (C) 2010  David Feil-Seifer [dfseifer@usc.edu], Edward T. Kaszubski [kaszubsk@usc.edu]
00035 #
00036 #       This program is free software; you can redistribute it and/or modify
00037 #       it under the terms of the GNU General Public License as published by
00038 #       the Free Software Foundation; either version 2 of the License, or
00039 #       (at your option) any later version.
00040 #       
00041 #       This program is distributed in the hope that it will be useful,
00042 #       but WITHOUT ANY WARRANTY; without even the implied warranty of
00043 #       MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00044 #       GNU General Public License for more details.
00045 #       
00046 #       You should have received a copy of the GNU General Public License
00047 #       along with this program; if not, write to the Free Software
00048 #       Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
00049 #       MA 02110-1301, USA.
00050 
00051 import roslib
00052 roslib.load_manifest('p2os_dashboard')
00053 
00054 import wx
00055 import wx.aui
00056 import wx.py.shell
00057 import rxtools
00058 import rxtools.cppwidgets as rxtools
00059 
00060 import robot_monitor
00061 from robot_monitor.robot_monitor_panel import RobotMonitorPanel
00062 from p2os_driver.msg import BatteryState, MotorState
00063 
00064 import std_msgs.msg
00065 import std_srvs.srv
00066 
00067 import rospy
00068 from roslib import rosenv
00069 
00070 from os import path
00071 import threading
00072 
00073 from status_control import StatusControl
00074 from rosout_frame import RosoutFrame
00075 
00076 from power_state_control import PowerStateControl
00077 
00078 from diagnostics_frame import DiagnosticsFrame
00079 
00080 class P2OSFrame(wx.Frame):
00081     _CONFIG_WINDOW_X="/Window/X"
00082     _CONFIG_WINDOW_Y="/Window/Y"
00083     
00084     def __init__(self, parent, id=wx.ID_ANY, title='P2OS Dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX|wx.STAY_ON_TOP):
00085         wx.Frame.__init__(self, parent, id, title, pos, size, style)
00086         
00087         wx.InitAllImageHandlers()
00088 
00089         self.motorStatePub = rospy.Publisher('cmd_motor_state', MotorState)
00090         self.motorStateSub = rospy.Subscriber("motor_state", MotorState, self.motorStateCallback)
00091 
00092         self.battStateSub = rospy.Subscriber("battery_state", BatteryState, self.battStateCallback)
00093 
00094         self.motorState = 0
00095         
00096         rospy.init_node('p2os_dashboard', anonymous=True)
00097         try:
00098             getattr(rxtools, "initRoscpp")
00099             rxtools.initRoscpp("p2os_dashboard_cpp", anonymous=True)
00100         except AttributeError:
00101             pass
00102         
00103         self.SetTitle('P2OS Dashboard (%s)'%rosenv.get_master_uri())
00104         
00105         icons_path = path.join(roslib.packages.get_pkg_dir('p2os_dashboard'), "icons/")
00106         
00107         sizer = wx.BoxSizer(wx.HORIZONTAL)
00108         self.SetSizer(sizer)
00109         
00110         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagnostic"), wx.HORIZONTAL)
00111         sizer.Add(static_sizer, 0)
00112         
00113         # Diagnostics
00114         self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "diag", True)
00115         self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
00116         static_sizer.Add(self._diagnostics_button, 0)
00117         
00118         # Rosout
00119         self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "rosout", True)
00120         self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
00121         static_sizer.Add(self._rosout_button, 0)
00122         
00123         # Motors
00124         self._motors_button = StatusControl(self, wx.ID_ANY, icons_path, "motor", True)
00125         self._motors_button.SetToolTip(wx.ToolTip("Motors"))
00126         static_sizer.Add(self._motors_button, 0)
00127         self._motors_button.Bind(wx.EVT_LEFT_DOWN, self.on_motors_clicked)
00128         
00129         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Battery"), wx.HORIZONTAL)
00130         sizer.Add(static_sizer, 0)
00131         
00132         # Battery State
00133         self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
00134         self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
00135         static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
00136         
00137         self._config = wx.Config("p2os_dashboard")
00138         
00139         self.Bind(wx.EVT_CLOSE, self.on_close)
00140         
00141         self.Layout()
00142         self.Fit()
00143         
00144         self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
00145         self._diagnostics_frame.Hide()
00146         self._diagnostics_frame.Center()
00147         self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
00148         
00149         self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
00150         self._rosout_frame.Hide()
00151         self._rosout_frame.Center()
00152         self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
00153         
00154         self.load_config()
00155         
00156         self._timer = wx.Timer(self)
00157         self.Bind(wx.EVT_TIMER, self.on_timer)
00158         self._timer.Start(1000)
00159         
00160         self.battery_msg_last_time = rospy.Time.now()
00161         #self.motor_msg_good = false;
00162         
00163     def __del__(self):
00164         self._dashboard_agg_sub.unregister()
00165 
00166     def battStateCallback(self, msg):
00167                 self.battery_msg_last_time = rospy.Time.now()
00168                 self._power_state_ctrl.set_power_state(msg)
00169 
00170     def motorStateCallback(self, msg):
00171                 self.motorState = msg.state
00172 
00173                 if(self.motorState):
00174                         self._motors_button.set_ok()
00175                         self._motors_button.SetToolTip(wx.ToolTip("Motors: Running"))
00176                 else:
00177                         self._motors_button.set_error()
00178                         self._motors_button.SetToolTip(wx.ToolTip("Motors: Halted"))
00179 
00180     def on_timer(self, evt):
00181       level = self._diagnostics_frame._diagnostics_panel.get_top_level_state()
00182       if (level == -1 or level == 3):
00183         if (self._diagnostics_button.set_stale()):
00184             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
00185       elif (level >= 2):
00186         if (self._diagnostics_button.set_error()):
00187             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
00188       elif (level == 1):
00189         if (self._diagnostics_button.set_warn()):
00190             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
00191       else:
00192         if (self._diagnostics_button.set_ok()):
00193             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
00194         
00195       
00196       if (rospy.Time.now() > self.battery_msg_last_time + rospy.Duration(5.0)):
00197                   #battery msg status is stale
00198                   #print "setting battery to stale"
00199                   self._power_state_ctrl.set_stale()
00200       
00201       self.update_rosout()
00202       self._power_state_ctrl.updateToolTip()
00203 
00204       if (rospy.is_shutdown()):
00205         self.Close()
00206         
00207     def on_diagnostics_clicked(self, evt):
00208       self._diagnostics_frame.Show()
00209       self._diagnostics_frame.Raise()
00210                 
00211     def on_rosout_clicked(self, evt):
00212       self._rosout_frame.Show()
00213       self._rosout_frame.Raise()
00214       
00215     def on_motors_clicked(self, evt):
00216       menu = wx.Menu()
00217       menu.Bind(wx.EVT_MENU, self.on_reset_motors, menu.Append(wx.ID_ANY, "Enable"))
00218       menu.Bind(wx.EVT_MENU, self.on_halt_motors, menu.Append(wx.ID_ANY, "Disable"))
00219       self._motors_button.toggle(True)
00220       self.PopupMenu(menu)
00221       self._motors_button.toggle(False)
00222       
00223     def on_reset_motors(self, evt):
00224 
00225       self.motorStatePub.publish(MotorState(1))
00226     
00227     def on_halt_motors(self, evt):
00228       self.motorStatePub.publish(MotorState(0))
00229                 
00230     def update_rosout(self):
00231       summary_dur = 30.0
00232       if (rospy.get_time() < 30.0):
00233           summary_dur = rospy.get_time() - 1.0
00234           
00235       if (summary_dur < 0):
00236           summary_dur = 0.0
00237     
00238       summary = self._rosout_frame.get_panel().getMessageSummary(summary_dur)
00239       
00240       if (summary.fatal or summary.error):
00241         self._rosout_button.set_error()
00242       elif (summary.warn):
00243         self._rosout_button.set_warn()
00244       else:
00245         self._rosout_button.set_ok()
00246         
00247         
00248       tooltip = ""
00249       if (summary.fatal):
00250         tooltip += "\nFatal: %s"%(summary.fatal)
00251       if (summary.error):
00252         tooltip += "\nError: %s"%(summary.error)
00253       if (summary.warn):
00254         tooltip += "\nWarn: %s"%(summary.warn)
00255       if (summary.info):
00256         tooltip += "\nInfo: %s"%(summary.info)
00257       if (summary.debug):
00258         tooltip += "\nDebug: %s"%(summary.debug)
00259       
00260       if (len(tooltip) == 0):
00261         tooltip = "Rosout: no recent activity"
00262       else:
00263         tooltip = "Rosout: recent activity:" + tooltip
00264     
00265       if (tooltip != self._rosout_button.GetToolTip().GetTip()):
00266           self._rosout_button.SetToolTip(wx.ToolTip(tooltip))
00267         
00268     def load_config(self):
00269       # Load our window options
00270       (x, y) = self.GetPositionTuple()
00271       (width, height) = self.GetSizeTuple()
00272       if (self._config.HasEntry(self._CONFIG_WINDOW_X)):
00273           x = self._config.ReadInt(self._CONFIG_WINDOW_X)
00274       if (self._config.HasEntry(self._CONFIG_WINDOW_Y)):
00275           y = self._config.ReadInt(self._CONFIG_WINDOW_Y)
00276       
00277       self.SetPosition((x, y))
00278       self.SetSize((width, height))
00279         
00280     def save_config(self):
00281       config = self._config
00282       
00283       (x, y) = self.GetPositionTuple()
00284       (width, height) = self.GetSizeTuple()
00285       config.WriteInt(self._CONFIG_WINDOW_X, x)
00286       config.WriteInt(self._CONFIG_WINDOW_Y, y)
00287       
00288       config.Flush()
00289         
00290     def on_close(self, event):
00291       self.save_config()
00292       self.Destroy()