turtlebot_dashboard: turtlebot_dashboard: turtlebot

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('turtlebot_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 import diagnostic_msgs.msg
00045 import std_msgs.msg
00046 import std_srvs.srv
00047 import turtlebot_node.msg
00048 import turtlebot_node.srv
00049 
00050 import rospy
00051 from rosgraph import rosenv
00052 
00053 from os import path
00054 import threading
00055 
00056 from breaker_control import BreakerControl
00057 from status_control import StatusControl
00058 from power_state_control import PowerStateControl
00059 from diagnostics_frame import DiagnosticsFrame
00060 from rosout_frame import RosoutFrame
00061 
00062 class TurtlebotFrame(wx.Frame):
00063     _CONFIG_WINDOW_X="/Window/X"
00064     _CONFIG_WINDOW_Y="/Window/Y"
00065     
00066     def __init__(self, parent, id=wx.ID_ANY, title='Turtlebot Dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX|wx.STAY_ON_TOP):
00067         wx.Frame.__init__(self, parent, id, title, pos, size, style)
00068         
00069         wx.InitAllImageHandlers()
00070         
00071         rospy.init_node('turtlebot_dashboard', anonymous=True)
00072         try:
00073             getattr(rxtools, "initRoscpp")
00074             rxtools.initRoscpp("turtlebot_dashboard_cpp", anonymous=True)
00075         except AttributeError:
00076             pass
00077         
00078         self.SetTitle('Turtlebot Dashboard (%s)'%rosenv.get_master_uri())
00079         
00080         icons_path = path.join(roslib.packages.get_pkg_dir('turtlebot_dashboard'), "icons/")
00081         
00082         sizer = wx.BoxSizer(wx.HORIZONTAL)
00083         self.SetSizer(sizer)
00084         
00085         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagnostic"), wx.HORIZONTAL)
00086         sizer.Add(static_sizer, 0)
00087         
00088         # Diagnostics
00089         self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "diag", True)
00090         self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
00091         static_sizer.Add(self._diagnostics_button, 0)
00092         
00093         # Rosout
00094         self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "rosout", True)
00095         self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
00096         static_sizer.Add(self._rosout_button, 0)
00097         
00098         # Motors
00099         self._motors_button = StatusControl(self, wx.ID_ANY, icons_path, "motor", True)
00100         self._motors_button.SetToolTip(wx.ToolTip("Mode"))
00101         static_sizer.Add(self._motors_button, 0)
00102         self._motors_button.Bind(wx.EVT_LEFT_DOWN, self.on_motors_clicked)
00103         
00104         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Breakers"), wx.HORIZONTAL)
00105         sizer.Add(static_sizer, 0)
00106         
00107         # Breakers
00108         breaker_names = ["Digital Out 1", "Digital Out 1", "Digital Out 2"]
00109         self._breaker_ctrls = []
00110         for i in xrange(0, 3):
00111           ctrl = BreakerControl(self, wx.ID_ANY, i, breaker_names[i], icons_path)
00112           ctrl.SetToolTip(wx.ToolTip("%s"%(breaker_names[i])))
00113           self._breaker_ctrls.append(ctrl)
00114           static_sizer.Add(ctrl, 0)
00115         
00116 #        static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Runstops"), wx.HORIZONTAL)
00117 #        sizer.Add(static_sizer, 0)
00118         
00119         # run-stop
00120 #        self._runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path, "runstop", False)
00121 #        self._runstop_ctrl.SetToolTip(wx.ToolTip("Physical Runstop: Unknown"))
00122 #        static_sizer.Add(self._runstop_ctrl, 0)
00123         
00124         # Wireless run-stop
00125 #        self._wireless_runstop_ctrl = StatusControl(self, wx.ID_ANY, icons_path, "runstop-wireless", False)
00126 #        self._wireless_runstop_ctrl.SetToolTip(wx.ToolTip("Wireless Runstop: Unknown"))
00127 #        static_sizer.Add(self._wireless_runstop_ctrl, 0)
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         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Laptop"), wx.HORIZONTAL)
00138         sizer.Add(static_sizer, 0)
00139         # Laptop Battery State
00140         self._power_state_ctrl_laptop = PowerStateControl(self, wx.ID_ANY, icons_path)
00141         self._power_state_ctrl_laptop.SetToolTip(wx.ToolTip("Laptop Battery: Stale"))
00142         static_sizer.Add(self._power_state_ctrl_laptop, 1, wx.EXPAND)
00143 
00144         self._config = wx.Config("turtlebot_dashboard")
00145         
00146         self.Bind(wx.EVT_CLOSE, self.on_close)
00147         
00148         self.Layout()
00149         self.Fit()
00150         
00151         self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
00152         self._diagnostics_frame.Hide()
00153         self._diagnostics_frame.Center()
00154         self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
00155         
00156         self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
00157         self._rosout_frame.Hide()
00158         self._rosout_frame.Center()
00159         self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
00160         
00161         self.load_config()
00162         
00163         self._timer = wx.Timer(self)
00164         self.Bind(wx.EVT_TIMER, self.on_timer)
00165         self._timer.Start(500)
00166     
00167         self._dashboard_agg_sub = rospy.Subscriber('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray, self.dashboard_callback)
00168         
00169         self._dashboard_message = None
00170         self._last_dashboard_message_time = 0.0
00171         
00172     def __del__(self):
00173         self._dashboard_agg_sub.unregister()
00174         
00175     def on_timer(self, evt):
00176       level = self._diagnostics_frame.get_top_level_state()
00177       if (level == -1 or level == 3):
00178         if (self._diagnostics_button.set_stale()):
00179             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
00180       elif (level >= 2):
00181         if (self._diagnostics_button.set_error()):
00182             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
00183       elif (level == 1):
00184         if (self._diagnostics_button.set_warn()):
00185             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
00186       else:
00187         if (self._diagnostics_button.set_ok()):
00188             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
00189         
00190       self.update_rosout()
00191       
00192       if (rospy.get_time() - self._last_dashboard_message_time > 5.0):
00193           self._motors_button.set_stale()
00194           self._power_state_ctrl.set_stale()
00195           self._power_state_ctrl_laptop.set_stale()
00196           [ctrl.reset() for ctrl in self._breaker_ctrls]
00197 #          self._runstop_ctrl.set_stale()
00198 #          self._wireless_runstop_ctrl.set_stale()
00199           ctrls = [self._motors_button, self._power_state_ctrl, self._power_state_ctrl_laptop]
00200           ctrls.extend(self._breaker_ctrls)
00201           for ctrl in ctrls:
00202               ctrl.SetToolTip(wx.ToolTip("No message received on dashboard_agg in the last 5 seconds"))
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_passive_mode, menu.Append(wx.ID_ANY, "Passive Mode"))
00218       menu.Bind(wx.EVT_MENU, self.on_safe_mode, menu.Append(wx.ID_ANY, "Safety Mode"))
00219       menu.Bind(wx.EVT_MENU, self.on_full_mode, menu.Append(wx.ID_ANY, "Full Mode"))
00220       self._motors_button.toggle(True)
00221       self.PopupMenu(menu)
00222       self._motors_button.toggle(False)
00223       
00224     def on_passive_mode(self, evt):
00225       passive = rospy.ServiceProxy("/turtlebot_node/set_operation_mode",turtlebot_node.srv.SetTurtlebotMode )
00226       try:
00227         passive(turtlebot_node.msg.TurtlebotSensorState.OI_MODE_PASSIVE)
00228       except rospy.ServiceException, e:
00229         wx.MessageBox("Failed to put the turtlebot in passive mode: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
00230 
00231     def on_safe_mode(self, evt):
00232       safe = rospy.ServiceProxy("/turtlebot_node/set_operation_mode",turtlebot_node.srv.SetTurtlebotMode)
00233       try:
00234         safe(turtlebot_node.msg.TurtlebotSensorState.OI_MODE_SAFE)
00235       except rospy.ServiceException, e:
00236         wx.MessageBox("Failed to put the turtlebot in safe mode: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
00237 
00238     def on_full_mode(self, evt):
00239       full = rospy.ServiceProxy("/turtlebot_node/set_operation_mode", turtlebot_node.srv.SetTurtlebotMode)
00240       try:
00241         full(turtlebot_node.msg.TurtlebotSensorState.OI_MODE_FULL)
00242       except rospy.ServiceException, e:
00243         wx.MessageBox("Failed to put the turtlebot in full mode: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
00244       
00245 
00246 
00247     def dashboard_callback(self, msg):
00248       wx.CallAfter(self.new_dashboard_message, msg)
00249       
00250     def new_dashboard_message(self, msg):
00251       self._dashboard_message = msg
00252       self._last_dashboard_message_time = rospy.get_time()
00253 
00254             
00255       battery_status = {}
00256       laptop_battery_status = {}
00257       breaker_status = {}
00258       op_mode = None
00259       for status in msg.status:
00260           if status.name == "/Power System/Battery":
00261               for value in status.values:
00262                   battery_status[value.key]=value.value
00263           if status.name == "/Power System/Laptop Battery":
00264               for value in status.values:
00265                   laptop_battery_status[value.key]=value.value
00266           if status.name == "/Mode/Operating Mode":
00267               op_mode=status.message
00268           if status.name == "/Digital IO/Digital Outputs":
00269               #print "got digital IO"
00270               for value in status.values:
00271                   breaker_status[value.key]=value.value
00272 
00273       if (battery_status):
00274         self._power_state_ctrl.set_power_state(battery_status)
00275       else:
00276         self._power_state_ctrl.set_stale()
00277 
00278       if (laptop_battery_status):
00279         self._power_state_ctrl_laptop.set_power_state(laptop_battery_status)
00280       else:
00281         self._power_state_ctrl_laptop.set_stale()
00282       
00283       if (op_mode):
00284         if (op_mode=='Full'):
00285           if (self._motors_button.set_ok()):
00286               self._motors_button.SetToolTip(wx.ToolTip("Mode: Full"))
00287         elif(op_mode=='Safe'):
00288           if (self._motors_button.set_warn()):
00289               self._motors_button.SetToolTip(wx.ToolTip("Mode: Safe"))
00290         elif(op_mode=='Passive'):
00291           if (self._motors_button.set_error()):
00292               self._motors_button.SetToolTip(wx.ToolTip("Mode: Passive"))
00293       else:
00294           if (self._motors_button.set_stale()):
00295               self._motors_button.SetToolTip(wx.ToolTip("Mode: Stale"))
00296 
00297       if (breaker_status):
00298           [ctrl.set_breaker_state(breaker_status) for ctrl in self._breaker_ctrls]
00299       
00300           
00301     def update_rosout(self):
00302       summary_dur = 30.0
00303       if (rospy.get_time() < 30.0):
00304           summary_dur = rospy.get_time() - 1.0
00305           
00306       if (summary_dur < 0):
00307           summary_dur = 0.0
00308     
00309       summary = self._rosout_frame.get_panel().getMessageSummary(summary_dur)
00310       
00311       if (summary.fatal or summary.error):
00312         self._rosout_button.set_error()
00313       elif (summary.warn):
00314         self._rosout_button.set_warn()
00315       else:
00316         self._rosout_button.set_ok()
00317         
00318         
00319       tooltip = ""
00320       if (summary.fatal):
00321         tooltip += "\nFatal: %s"%(summary.fatal)
00322       if (summary.error):
00323         tooltip += "\nError: %s"%(summary.error)
00324       if (summary.warn):
00325         tooltip += "\nWarn: %s"%(summary.warn)
00326       if (summary.info):
00327         tooltip += "\nInfo: %s"%(summary.info)
00328       if (summary.debug):
00329         tooltip += "\nDebug: %s"%(summary.debug)
00330       
00331       if (len(tooltip) == 0):
00332         tooltip = "Rosout: no recent activity"
00333       else:
00334         tooltip = "Rosout: recent activity:" + tooltip
00335     
00336       if (tooltip != self._rosout_button.GetToolTip().GetTip()):
00337           self._rosout_button.SetToolTip(wx.ToolTip(tooltip))
00338         
00339     def load_config(self):
00340       # Load our window options
00341       (x, y) = self.GetPositionTuple()
00342       (width, height) = self.GetSizeTuple()
00343       if (self._config.HasEntry(self._CONFIG_WINDOW_X)):
00344           x = self._config.ReadInt(self._CONFIG_WINDOW_X)
00345       if (self._config.HasEntry(self._CONFIG_WINDOW_Y)):
00346           y = self._config.ReadInt(self._CONFIG_WINDOW_Y)
00347       
00348       self.SetPosition((x, y))
00349       self.SetSize((width, height))
00350         
00351     def save_config(self):
00352       config = self._config
00353       
00354       (x, y) = self.GetPositionTuple()
00355       (width, height) = self.GetSizeTuple()
00356       config.WriteInt(self._CONFIG_WINDOW_X, x)
00357       config.WriteInt(self._CONFIG_WINDOW_Y, y)
00358       
00359       config.Flush()
00360         
00361     def on_close(self, event):
00362       self.save_config()
00363       
00364       self.Destroy()
00365
turtlebot_frame.py
