lwr_dashboard: main_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 import roslib
00034 roslib.load_manifest('lwr_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 
00048 import rospy
00049 from roslib import rosenv
00050 
00051 from os import path
00052 import threading
00053 
00054 from status_control import StatusControl
00055 from fri_control import FRIControl
00056 from robot_control import RobotControl
00057 from diagnostics_frame import DiagnosticsFrame
00058 from rosout_frame import RosoutFrame
00059 
00060 import re
00061 
00062 class MainFrame(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='Kuka LWR 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('lwr_dashboard', anonymous=True)
00072         try:
00073             getattr(rxtools, "initRoscpp")
00074             rxtools.initRoscpp("lwr_dashboard_cpp", anonymous=True)
00075         except AttributeError:
00076             print "rxtools error"
00077             pass
00078 
00079         self.SetTitle('Kuka LWR Dashboard (%s)' % rosenv.get_master_uri())
00080 
00081         icons_path = path.join(roslib.packages.get_pkg_dir('lwr_dashboard'), "icons/")
00082 
00083 
00084         self._robots = rospy.get_param("robots", [""])
00085 
00086         if (len(self._robots) == 0):
00087             self._has_many = False
00088         else:
00089             self._has_many = True
00090 
00091 
00092         sizer = wx.BoxSizer(wx.HORIZONTAL)
00093         self.SetSizer(sizer)
00094 
00095         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, " ROS state "), wx.HORIZONTAL)
00096         sizer.Add(static_sizer, 0)
00097 
00098         # Diagnostics
00099         self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "btn_diag", True)
00100         self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
00101         static_sizer.Add(self._diagnostics_button, 0)
00102 
00103         # Rosout
00104         self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "btn_rosout", True)
00105         self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
00106         static_sizer.Add(self._rosout_button, 0)
00107 
00108         self.FRI_COMMAND = 1
00109         self.FRI_MONITOR = 2
00110 
00111         self._fri_state_button = {}
00112         self._fri_control = {}
00113         self._fri_mode_topic = {}
00114         self._robot_control = {}
00115 
00116         i = 0
00117         for robot in self._robots:
00118                         
00119             print "Preparing robot: '%s'"%(robot)
00120             robot_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, " %s " % robot), wx.HORIZONTAL)
00121 
00122 
00123             # FRI State
00124             static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, " FRI "), wx.HORIZONTAL)
00125             robot_sizer.Add(static_sizer, 0)
00126 
00127             self._fri_state_button[robot] = StatusControl(self, wx.ID_ANY, icons_path, "btn_state", True)
00128             self._fri_state_button[robot].SetToolTip(wx.ToolTip("FRI state"))
00129            
00130             self._fri_state_button[robot].Bind(wx.EVT_BUTTON, lambda x, r=robot: self.on_fri_state_clicked(x, r))
00131             static_sizer.Add(self._fri_state_button[robot], 0)
00132 
00133             self._fri_control[robot] = FRIControl(self, wx.ID_ANY, icons_path)
00134             self._fri_control[robot].SetToolTip(wx.ToolTip("FRI: Stale"))
00135             static_sizer.Add(self._fri_control[robot], 1, wx.EXPAND)
00136 
00137 
00138             self._fri_mode_topic[robot] = rospy.Publisher("%sarm_controller/fri_set_mode" % robot, std_msgs.msg.Int32)
00139 
00140             # Robot State
00141             static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, " Robot "), wx.HORIZONTAL)
00142             robot_sizer.Add(static_sizer, 0)
00143 
00144             self._robot_control[robot] = RobotControl(self, wx.ID_ANY, icons_path)
00145             self._robot_control[robot].SetToolTip(wx.ToolTip("Robot: Stale"))
00146             static_sizer.Add(self._robot_control[robot], 1, wx.EXPAND)
00147 
00148             sizer.Add(robot_sizer, 0)
00149 
00150             i = i + 1
00151 
00152 
00153         self._config = wx.Config("elektron_dashboard")
00154 
00155         self.Bind(wx.EVT_CLOSE, self.on_close)
00156 
00157         self.Layout()
00158         self.Fit()
00159 
00160         self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
00161         self._diagnostics_frame.Hide()
00162         self._diagnostics_frame.Center()
00163         self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
00164 
00165         self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
00166         self._rosout_frame.Hide()
00167         self._rosout_frame.Center()
00168         self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
00169 
00170         self.load_config()
00171 
00172         self._timer = wx.Timer(self)
00173         self.Bind(wx.EVT_TIMER, self.on_timer)
00174         self._timer.Start(500)
00175 
00176         self._topic = "diagnostic"
00177 
00178         self._subs = []
00179         sub = rospy.Subscriber(self._topic, diagnostic_msgs.msg.DiagnosticArray, self.dashboard_callback)
00180         self._subs.append(sub)
00181 
00182         print "Subs.size %d" % len(self._subs)
00183 
00184         self._dashboard_message = None
00185         self._last_dashboard_message_time = 0.0
00186 
00187     def __del__(self):
00188         for sub in self._subs:
00189             sub.unregister()
00190 
00191 
00192     def on_timer(self, evt):
00193       level = self._diagnostics_frame._diagnostics_panel.get_top_level_state()
00194       if (level == -1 or level == 3):
00195          if (self._diagnostics_button.set_stale()):
00196             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
00197       elif (level >= 2):
00198          if (self._diagnostics_button.set_error()):
00199             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
00200       elif (level == 1):
00201          if (self._diagnostics_button.set_warn()):
00202             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
00203       else:
00204          if (self._diagnostics_button.set_ok()):
00205             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
00206 
00207       self.update_rosout()
00208 
00209       if (rospy.get_time() - self._last_dashboard_message_time > 5.0):
00210           for ctrl in self._fri_control.values():
00211               ctrl.set_stale()
00212 
00213           for ctrl in self._robot_control.values():
00214               ctrl.set_stale()
00215 
00216           for ctrl in self._fri_state_button.values():
00217               ctrl.set_stale()
00218 
00219 
00220       if (rospy.is_shutdown()):
00221         self.Close()
00222 
00223     def on_diagnostics_clicked(self, evt):
00224         self._diagnostics_frame.Show()
00225         self._diagnostics_frame.Raise()
00226 
00227     def on_rosout_clicked(self, evt):
00228         self._rosout_frame.Show()
00229         self._rosout_frame.Raise()
00230 
00231     def dashboard_callback(self, msg):
00232       wx.CallAfter(self.new_dashboard_message, msg)
00233 
00234 
00235     def new_dashboard_message(self, msg):
00236       self._dashboard_message = msg
00237       self._last_dashboard_message_time = rospy.get_time()
00238 
00239       fri_status = {}
00240       robot_status = {}
00241 
00242       for status in msg.status:
00243 
00244           parts = status.name.split(" ");
00245 
00246           robot = parts[0]
00247           dtype = parts[1]
00248 
00249           if dtype == "FRI":
00250               for value in status.values:
00251                   fri_status[value.key] = value.value
00252           if dtype == "robot":
00253               for value in status.values:
00254                   robot_status[value.key] = value.value
00255 
00256 
00257       if (fri_status):
00258         self._fri_control[robot].set_state(fri_status)
00259         self.update_fri(fri_status, robot)
00260       else:
00261         self._fri_control[robot].set_stale()
00262 
00263       if (robot_status):
00264         self._robot_control[robot].set_state(robot_status)
00265       else:
00266         self._robot_control[robot].set_stale()
00267 
00268 
00269 
00270     ###################################################################
00271     # FRI related stuff
00272     ###################################################################
00273 
00274     def update_fri(self, msg, robot):
00275         if (msg["State"] == "command"):
00276             self._fri_state_button[robot].set_ok()
00277         else:
00278             self._fri_state_button[robot].set_warn()
00279 
00280     def on_fri_state_clicked(self, ev, robot):
00281         menu = wx.Menu()
00282         menu.Bind(wx.EVT_MENU, lambda x, r=robot: self.on_monitor(x, r), menu.Append(wx.ID_ANY, "Monitor"))
00283         menu.Bind(wx.EVT_MENU, lambda x, r=robot: self.on_command(x, r), menu.Append(wx.ID_ANY, "Command"))
00284 
00285         self.PopupMenu(menu)
00286 
00287     def on_monitor(self, evt, robot):
00288         self.set_mode(self.FRI_MONITOR, robot)
00289 
00290     def on_command(self, evt, robot):
00291         self.set_mode(self.FRI_COMMAND, robot)
00292 
00293     def set_mode(self, mode, robot):
00294         self._fri_mode_topic[robot].publish(std_msgs.msg.Int32(mode))
00295 
00296 
00297 
00298 
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         tooltip = ""
00319         if (summary.fatal):
00320             tooltip += "\nFatal: %s"%(summary.fatal)
00321         if (summary.error):
00322             tooltip += "\nError: %s"%(summary.error)
00323         if (summary.warn):
00324             tooltip += "\nWarn: %s"%(summary.warn)
00325         if (summary.info):
00326             tooltip += "\nInfo: %s"%(summary.info)
00327         if (summary.debug):
00328             tooltip += "\nDebug: %s"%(summary.debug)
00329 
00330         if (len(tooltip) == 0):
00331             tooltip = "Rosout: no recent activity"
00332         else:
00333             tooltip = "Rosout: recent activity:" + tooltip
00334 
00335         if (tooltip != self._rosout_button.GetToolTip().GetTip()):
00336             self._rosout_button.SetToolTip(wx.ToolTip(tooltip))
00337 
00338 
00339 
00340     def load_config(self):
00341       # Load our window options
00342       (x, y) = self.GetPositionTuple()
00343       (width, height) = self.GetSizeTuple()
00344       if (self._config.HasEntry(self._CONFIG_WINDOW_X)):
00345           x = self._config.ReadInt(self._CONFIG_WINDOW_X)
00346       if (self._config.HasEntry(self._CONFIG_WINDOW_Y)):
00347           y = self._config.ReadInt(self._CONFIG_WINDOW_Y)
00348 
00349       self.SetPosition((x, y))
00350       self.SetSize((width, height))
00351 
00352 
00353 
00354     def save_config(self):
00355       config = self._config
00356 
00357       (x, y) = self.GetPositionTuple()
00358       (width, height) = self.GetSizeTuple()
00359       config.WriteInt(self._CONFIG_WINDOW_X, x)
00360       config.WriteInt(self._CONFIG_WINDOW_Y, y)
00361 
00362       config.Flush()
00363 
00364     def on_close(self, event):
00365       self.save_config()
00366       print "Closing..."
00367       self.Destroy()