nao_dashboard: nao_dashboard: nao

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 #
00034 # Ported from pr2_dashboard: Stefan Osswald, University of Freiburg, 2011.
00035 #
00036 
00037 import roslib
00038 roslib.load_manifest('nao_dashboard')
00039 
00040 import wx
00041 import wx.aui
00042 import wx.py.shell
00043 import rxtools
00044 import rxtools.cppwidgets as rxtools
00045 
00046 import dbus, gobject, dbus.glib
00047 from diagnostic_msgs.msg import *
00048 
00049 from nao_msgs.msg import BodyPoseAction, BodyPoseGoal
00050 import actionlib
00051 import std_srvs.srv
00052 
00053 ID_INIT_POSE = wx.NewId()
00054 ID_SIT_DOWN = wx.NewId()
00055 ID_REMOVE_STIFFNESS = wx.NewId()
00056 
00057 #import avahi
00058 class avahi:
00059     DBUS_NAME = "org.freedesktop.Avahi"
00060     DBUS_INTERFACE_SERVER = DBUS_NAME + ".Server"
00061     DBUS_PATH_SERVER = "/"
00062     DBUS_INTERFACE_ENTRY_GROUP = DBUS_NAME + ".EntryGroup"
00063     DBUS_INTERFACE_DOMAIN_BROWSER = DBUS_NAME + ".DomainBrowser"
00064     DBUS_INTERFACE_SERVICE_TYPE_BROWSER = DBUS_NAME + ".ServiceTypeBrowser"
00065     DBUS_INTERFACE_SERVICE_BROWSER = DBUS_NAME + ".ServiceBrowser"
00066     DBUS_INTERFACE_ADDRESS_RESOLVER = DBUS_NAME + ".AddressResolver"
00067     DBUS_INTERFACE_HOST_NAME_RESOLVER = DBUS_NAME + ".HostNameResolver"
00068     DBUS_INTERFACE_SERVICE_RESOLVER = DBUS_NAME + ".ServiceResolver"
00069     DBUS_INTERFACE_RECORD_BROWSER = DBUS_NAME + ".RecordBrowser"    
00070     PROTO_UNSPEC, PROTO_INET, PROTO_INET6  = -1, 0, 1
00071     IF_UNSPEC = -1
00072     LOOKUP_RESULT_CACHED = 1
00073     LOOKUP_RESULT_WIDE_AREA = 2
00074     LOOKUP_RESULT_MULTICAST = 4
00075     LOOKUP_RESULT_LOCAL = 8
00076     LOOKUP_RESULT_OUR_OWN = 16
00077     LOOKUP_RESULT_STATIC = 32
00078 
00079 
00080 import robot_monitor
00081 from robot_monitor.robot_monitor_panel import RobotMonitorPanel
00082 import std_msgs.msg
00083 import std_srvs.srv
00084 
00085 import rospy
00086 from roslib import rosenv
00087 
00088 from os import path
00089 import threading
00090 
00091 from status_control import StatusControl
00092 from power_state_control import PowerStateControl
00093 from diagnostics_frame import DiagnosticsFrame
00094 from rosout_frame import RosoutFrame
00095 
00096 class NaoFrame(wx.Frame):
00097     _CONFIG_WINDOW_X="/Window/X"
00098     _CONFIG_WINDOW_Y="/Window/Y"
00099     
00100     def __init__(self, parent, id=wx.ID_ANY, title='Nao Dashboard', pos=wx.DefaultPosition, size=(400, 50), style=wx.CAPTION|wx.CLOSE_BOX|wx.STAY_ON_TOP):
00101         wx.Frame.__init__(self, parent, id, title, pos, size, style)
00102         
00103         wx.InitAllImageHandlers()
00104         
00105         rospy.init_node('nao_dashboard', anonymous=True)
00106         try:
00107             getattr(rxtools, "initRoscpp")
00108             rxtools.initRoscpp("nao_dashboard_cpp", anonymous=True)
00109         except AttributeError:
00110             pass
00111         
00112         self.SetTitle('Nao Dashboard (%s)'%rosenv.get_master_uri())
00113         
00114         icons_path = path.join(roslib.packages.get_pkg_dir('nao_dashboard'), "icons/")
00115         
00116         sizer = wx.BoxSizer(wx.HORIZONTAL)
00117         self.SetSizer(sizer)
00118         
00119         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Robot"), wx.HORIZONTAL)
00120         sizer.Add(static_sizer, 0)
00121         self._robot_combobox = wx.ComboBox(self, wx.ID_ANY, "", choices = [])
00122         static_sizer.Add(self._robot_combobox, 0)
00123         
00124         gobject.threads_init()
00125         dbus.glib.threads_init() 
00126         self.robots = []
00127         self.sys_bus = dbus.SystemBus()
00128         self.avahi_server = dbus.Interface(self.sys_bus.get_object(avahi.DBUS_NAME, '/'), avahi.DBUS_INTERFACE_SERVER)
00129 
00130         self.sbrowser = dbus.Interface(self.sys_bus.get_object(avahi.DBUS_NAME, self.avahi_server.ServiceBrowserNew(avahi.IF_UNSPEC,
00131             avahi.PROTO_INET, '_naoqi._tcp', 'local', dbus.UInt32(0))), avahi.DBUS_INTERFACE_SERVICE_BROWSER)
00132 
00133         self.sbrowser.connect_to_signal("ItemNew", self.avahiNewItem)
00134         self.sbrowser.connect_to_signal("ItemRemove", self.avahiItemRemove)
00135 
00136         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Diagnostic"), wx.HORIZONTAL)
00137         sizer.Add(static_sizer, 0)
00138         
00139         # Diagnostics
00140         self._diagnostics_button = StatusControl(self, wx.ID_ANY, icons_path, "diag", True)
00141         self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics"))
00142         static_sizer.Add(self._diagnostics_button, 0)
00143         
00144         # Rosout
00145         self._rosout_button = StatusControl(self, wx.ID_ANY, icons_path, "rosout", True)
00146         self._rosout_button.SetToolTip(wx.ToolTip("Rosout"))
00147         static_sizer.Add(self._rosout_button, 0)
00148         
00149         # Joint temperature
00150         self._temp_joint_button = StatusControl(self, wx.ID_ANY, icons_path, "temperature_joints", True)
00151         self._temp_joint_button.SetToolTip(wx.ToolTip("Joint temperatures"))
00152         static_sizer.Add(self._temp_joint_button, 0)
00153 
00154         # CPU temperature
00155         self._temp_head_button = StatusControl(self, wx.ID_ANY, icons_path, "temperature_head", True)
00156         self._temp_head_button.SetToolTip(wx.ToolTip("CPU temperature"))
00157         static_sizer.Add(self._temp_head_button, 0)
00158 
00159         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Stiffness"), wx.HORIZONTAL)
00160         sizer.Add(static_sizer, 0)
00161 
00162         # Motors
00163         self._motors_button = StatusControl(self, wx.ID_ANY, icons_path, "motor", True)
00164         self._motors_button.SetToolTip(wx.ToolTip("Stiffness"))
00165         self._motors_button._ok = (wx.Bitmap(path.join(icons_path, "stiffness-off-untoggled.png"), wx.BITMAP_TYPE_PNG), 
00166                     wx.Bitmap(path.join(icons_path, "stiffness-off-toggled.png"), wx.BITMAP_TYPE_PNG))
00167         self._motors_button._warn = (wx.Bitmap(path.join(icons_path, "stiffness-partially-untoggled.png"), wx.BITMAP_TYPE_PNG), 
00168                     wx.Bitmap(path.join(icons_path, "stiffness-partially-toggled.png"), wx.BITMAP_TYPE_PNG))
00169         self._motors_button._error = (wx.Bitmap(path.join(icons_path, "stiffness-on-untoggled.png"), wx.BITMAP_TYPE_PNG), 
00170                      wx.Bitmap(path.join(icons_path, "stiffness-on-toggled.png"), wx.BITMAP_TYPE_PNG))
00171         self._motors_button._stale = (wx.Bitmap(path.join(icons_path, "stiffness-stale-untoggled.png"), wx.BITMAP_TYPE_PNG), 
00172                     wx.Bitmap(path.join(icons_path, "stiffness-stale-toggled.png"), wx.BITMAP_TYPE_PNG))
00173         static_sizer.Add(self._motors_button, 0)
00174         self._motors_button.Bind(wx.EVT_LEFT_DOWN, self.on_motors_clicked)
00175                 
00176         static_sizer = wx.StaticBoxSizer(wx.StaticBox(self, wx.ID_ANY, "Battery"), wx.HORIZONTAL)
00177         sizer.Add(static_sizer, 0)
00178         
00179         # Battery State
00180         self._power_state_ctrl = PowerStateControl(self, wx.ID_ANY, icons_path)
00181         self._power_state_ctrl.SetToolTip(wx.ToolTip("Battery: Stale"))
00182         static_sizer.Add(self._power_state_ctrl, 1, wx.EXPAND)
00183         
00184         self._config = wx.Config("nao_dashboard")
00185         
00186         self.Bind(wx.EVT_CLOSE, self.on_close)
00187         
00188         self.Layout()
00189         self.Fit()
00190         
00191         self._diagnostics_frame = DiagnosticsFrame(self, wx.ID_ANY, "Diagnostics")
00192         self._diagnostics_frame.Hide()
00193         self._diagnostics_frame.Center()
00194         self._diagnostics_button.Bind(wx.EVT_BUTTON, self.on_diagnostics_clicked)
00195         
00196         self._rosout_frame = RosoutFrame(self, wx.ID_ANY, "Rosout")
00197         self._rosout_frame.Hide()
00198         self._rosout_frame.Center()
00199         self._rosout_button.Bind(wx.EVT_BUTTON, self.on_rosout_clicked)
00200         
00201         self.load_config()
00202         
00203         self._timer = wx.Timer(self)
00204         self.Bind(wx.EVT_TIMER, self.on_timer)
00205         self._timer.Start(500)
00206     
00207         self._agg_sub = rospy.Subscriber('diagnostics_agg', DiagnosticArray, self.diagnostic_callback)
00208         self._last_diagnostics_message_time = 0.0
00209         self.bodyPoseClient = actionlib.SimpleActionClient('body_pose', BodyPoseAction)
00210         self.stiffnessEnableClient = rospy.ServiceProxy("body_stiffness/enable", std_srvs.srv.Empty)
00211         self.stiffnessDisableClient = rospy.ServiceProxy("body_stiffness/disable", std_srvs.srv.Empty)
00212         
00213         
00214     def __del__(self):
00215         self._dashboard_agg_sub.unregister()
00216         
00217     def on_timer(self, evt):
00218       level = self._diagnostics_frame._diagnostics_panel.get_top_level_state()
00219       if (level == -1 or level == 3):
00220         if (self._diagnostics_button.set_stale()):
00221             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Stale"))
00222       elif (level >= 2):
00223         if (self._diagnostics_button.set_error()):
00224             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Error"))
00225       elif (level == 1):
00226         if (self._diagnostics_button.set_warn()):
00227             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: Warning"))
00228       else:
00229         if (self._diagnostics_button.set_ok()):
00230             self._diagnostics_button.SetToolTip(wx.ToolTip("Diagnostics: OK"))
00231         
00232       self.update_rosout()
00233       
00234       if (rospy.get_time() - self._last_diagnostics_message_time > 5.0):
00235           ctrls = [self._motors_button, self._power_state_ctrl, self._temp_head_button, self._temp_joint_button]
00236           for ctrl in ctrls:
00237               ctrl.set_stale()
00238               ctrl.SetToolTip(wx.ToolTip("No message received on diagnostics_agg in the last 5 seconds"))
00239         
00240       if (rospy.is_shutdown()):
00241         self.Close()
00242         
00243     def on_diagnostics_clicked(self, evt):
00244       self._diagnostics_frame.Show()
00245       self._diagnostics_frame.Raise()
00246       
00247     def on_rosout_clicked(self, evt):
00248       self._rosout_frame.Show()
00249       self._rosout_frame.Raise()
00250       
00251     def on_motors_clicked(self, evt):      
00252       menu = wx.Menu()
00253       menu.Append(ID_INIT_POSE, "Init pose")
00254       menu.Append(ID_SIT_DOWN, "Sit down && remove stiffness")
00255       menu.Append(ID_REMOVE_STIFFNESS, "Remove stiffness immediately")
00256       wx.EVT_MENU(self, ID_INIT_POSE, self.on_init_pose)
00257       wx.EVT_MENU(self, ID_SIT_DOWN, self.on_sit_down)
00258       wx.EVT_MENU(self, ID_REMOVE_STIFFNESS, self.on_remove_stiffness)
00259                         
00260       #subscriberFound = ... 
00261       #menu.Enable(ID_INIT_POSE, subscriberFound);
00262       #menu.Enable(ID_SIT_DOWN, subscriberFound);
00263       #menu.Enable(ID_REMOVE_STIFFNESS, subscriberFound);      
00264       self._motors_button.toggle(True)
00265       self.PopupMenu(menu)
00266       self._motors_button.toggle(False)
00267       
00268     def on_init_pose(self, evt):
00269         self.stiffnessEnableClient.call()
00270         self.bodyPoseClient.send_goal_and_wait(BodyPoseGoal(pose_name = 'init'))
00271   
00272     def on_sit_down(self, evt):
00273         self.bodyPoseClient.send_goal_and_wait(BodyPoseGoal(pose_name = 'crouch'))
00274         state = self.bodyPoseClient.get_state()
00275         if state == actionlib.GoalStatus.SUCCEEDED:
00276             self.stiffnessDisableClient.call()
00277         else:
00278             wx.MessageBox('crouch pose did not succeed: %s - cannot remove stiffness' % self.bodyPoseClient.get_goal_status_text(), 'Error')
00279             rospy.logerror("crouch pose did not succeed: %s", self.bodyPoseClient.get_goal_status_text())
00280 
00281   
00282     def on_remove_stiffness(self, evt):
00283       msg = wx.MessageDialog(self, 'Caution: Robot may fall. Continue to remove stiffness?', 'Warning', wx.YES_NO | wx.NO_DEFAULT | wx.CENTER | wx.ICON_EXCLAMATION)
00284       if(msg.ShowModal() == wx.ID_YES):
00285           self.stiffnessDisableClient.call()
00286     
00287     def on_halt_motors(self, evt):
00288       halt = rospy.ServiceProxy("pr2_etherCAT/halt_motors", std_srvs.srv.Empty)
00289        
00290       try:
00291         halt()
00292       except rospy.ServiceException, e:
00293         wx.MessageBox("Failed to halt the motors: service call failed with error: %s"%(e), "Error", wx.OK|wx.ICON_ERROR)
00294       
00295     def diagnostic_callback(self, msg):
00296       wx.CallAfter(self.new_diagnostic_message, msg)
00297       
00298     def new_diagnostic_message(self, msg):
00299         self._last_diagnostics_message_time = rospy.get_time()
00300         for status in msg.status:
00301             if status.name == '/Nao/Joints':
00302                 highestTemp = ""
00303                 lowestStiff = -1.0
00304                 highestStiff = -1.0
00305                 hotJoints = ""
00306                 for kv in status.values:
00307                      if kv.key == 'Highest Temperature':
00308                          highestTemp = " (" + kv.value + "deg C)"
00309                      elif kv.key == 'Highest Stiffness':
00310                          highestStiff = float(kv.value)
00311                      elif kv.key == 'Lowest Stiffness without Hands':
00312                          lowestStiff = float(kv.value)
00313                      elif kv.key == 'Hot Joints':
00314                          hotJoints = str(kv.value)
00315                 self.set_buttonStatus(self._temp_joint_button, status, "Joints: ", "%s %s"%(highestTemp, hotJoints))
00316                 if(lowestStiff < 0.0 or highestStiff < 0.0):
00317                     self._motors_button.set_stale()
00318                     self._motors_button.SetToolTip(wx.ToolTip("Stale"))
00319                 elif(lowestStiff > 0.9):
00320                     self._motors_button.set_error()
00321                     self._motors_button.SetToolTip(wx.ToolTip("Stiffness on"))
00322                 elif(highestStiff < 0.05):
00323                     self._motors_button.set_ok()
00324                     self._motors_button.SetToolTip(wx.ToolTip("Stiffness off"))
00325                 else:
00326                     self._motors_button.set_warn()
00327                     self._motors_button.SetToolTip(wx.ToolTip("Stiffness partially on (between %f and %f)" % (lowestStiff, highestStiff)))
00328             elif status.name == '/Nao/CPU':
00329                 self.set_buttonStatus(self._temp_head_button, status, "CPU temperature: ")
00330             elif status.name == '/Nao/Battery/Battery':
00331                 if status.level == 3:
00332                     self._power_state_ctrl.set_stale()
00333                 else:
00334                     self._power_state_ctrl.set_power_state(status.values)
00335                     
00336     def set_buttonStatus(self, button, status, statusPrefix = "", statusSuffix = ""):
00337         statusString = "Unknown"
00338         if status.level == DiagnosticStatus.OK:
00339             button.set_ok()
00340             statusString = "OK"
00341         elif status.level == DiagnosticStatus.WARN:
00342             button.set_warn()
00343             statusString = "Warn"
00344         elif status.level == DiagnosticStatus.ERROR:
00345             button.set_error()
00346             statusString = "Error"
00347         elif status.level == 3:
00348             button.set_stale()
00349             statusString = "Stale"
00350         button.SetToolTip(wx.ToolTip(statusPrefix + statusString + statusSuffix))
00351                
00352     def update_rosout(self):
00353       summary_dur = 30.0
00354       if (rospy.get_time() < 30.0):
00355           summary_dur = rospy.get_time() - 1.0
00356           
00357       if (summary_dur < 0):
00358           summary_dur = 0.0
00359     
00360       summary = self._rosout_frame.get_panel().getMessageSummary(summary_dur)
00361       
00362       if (summary.fatal or summary.error):
00363         self._rosout_button.set_error()
00364       elif (summary.warn):
00365         self._rosout_button.set_warn()
00366       else:
00367         self._rosout_button.set_ok()
00368         
00369         
00370       tooltip = ""
00371       if (summary.fatal):
00372         tooltip += "\nFatal: %s"%(summary.fatal)
00373       if (summary.error):
00374         tooltip += "\nError: %s"%(summary.error)
00375       if (summary.warn):
00376         tooltip += "\nWarn: %s"%(summary.warn)
00377       if (summary.info):
00378         tooltip += "\nInfo: %s"%(summary.info)
00379       if (summary.debug):
00380         tooltip += "\nDebug: %s"%(summary.debug)
00381       
00382       if (len(tooltip) == 0):
00383         tooltip = "Rosout: no recent activity"
00384       else:
00385         tooltip = "Rosout: recent activity:" + tooltip
00386     
00387       if (tooltip != self._rosout_button.GetToolTip().GetTip()):
00388           self._rosout_button.SetToolTip(wx.ToolTip(tooltip))
00389         
00390     def load_config(self):
00391       # Load our window options
00392       (x, y) = self.GetPositionTuple()
00393       (width, height) = self.GetSizeTuple()
00394       if (self._config.HasEntry(self._CONFIG_WINDOW_X)):
00395           x = self._config.ReadInt(self._CONFIG_WINDOW_X)
00396       if (self._config.HasEntry(self._CONFIG_WINDOW_Y)):
00397           y = self._config.ReadInt(self._CONFIG_WINDOW_Y)
00398       
00399       self.SetPosition((x, y))
00400       self.SetSize((width, height))
00401         
00402     def save_config(self):
00403       config = self._config
00404       
00405       (x, y) = self.GetPositionTuple()
00406       (width, height) = self.GetSizeTuple()
00407       config.WriteInt(self._CONFIG_WINDOW_X, x)
00408       config.WriteInt(self._CONFIG_WINDOW_Y, y)
00409       
00410       config.Flush()
00411         
00412     def on_close(self, event):
00413       self.save_config()
00414       
00415       self.Destroy()
00416       
00417     def avahiNewItem(self, interface, protocol, name, stype, domain, flags):
00418         self.avahi_server.ResolveService(interface, protocol, name, stype, 
00419             domain, avahi.PROTO_INET, dbus.UInt32(0), 
00420             reply_handler=self.service_resolved, error_handler=self.print_error)
00421         pass
00422     
00423     def avahiItemRemove(self, interface, protocol, name, stype, domain, flags):
00424         print "Remove"
00425         for robot in self.robots:
00426             if robot['name'] == str(name) and robot['address'] == str(address) and robot['port'] == int(port):
00427                 self.robots.remove(robot)
00428         updateRobotCombobox();
00429       
00430     def service_resolved(self, interface, protocol, name, type, domain, host, aprotocol, address, port, txt, flags):
00431         self.robots.append({'name': str(name), 'address': str(address), 'port': int(port)})
00432         self.updateRobotCombobox()
00433         
00434     def updateRobotCombobox(self):
00435         selected = self._robot_combobox.GetValue()
00436         self._robot_combobox.Clear()
00437         id = -1
00438         for robot in self.robots:
00439             text = str(robot)
00440             text = "%s (%s:%d)" % (robot['name'], robot['address'], robot['port'])
00441             self._robot_combobox.Append(text)
00442             if(text == selected):
00443                 id = self._robot_combobox.GetCount()-1;
00444             
00445         if(self._robot_combobox.GetCount() == 1):
00446             self._robot_combobox.SetSelection(0)
00447         elif(id > -1):
00448             self._robot_combobox.SetSelection(id)
00449 
00450         
00451     def print_error(self, *args):
00452         print 'error_handler'
00453         print args
00454             
00455
nao_frame.py
