frame.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2008, Willow Garage, Inc.
00004 # Copyright (c) 2014, Aldebaran Robotics (c)
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 #
00035 # Ported from pr2_dashboard: Stefan Osswald, University of Freiburg, 2011.
00036 #
00037 
00038 import roslib
00039 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
00040 
00041 from naoqi_bridge_msgs.msg import BodyPoseAction, BodyPoseGoal
00042 import actionlib
00043 
00044 import rospy
00045 from rosgraph import rosenv
00046 
00047 from .status_control import StatusControl
00048 from .power_state_control import PowerStateControl
00049 from .motors import Motors
00050 from .avahi import AvahiWidget
00051 
00052 from rqt_robot_dashboard.dashboard import Dashboard
00053 from rqt_robot_dashboard.monitor_dash_widget import MonitorDashWidget
00054 from rqt_robot_dashboard.console_dash_widget import ConsoleDashWidget
00055 
00056 class NAOqiDashboard(Dashboard):
00057     
00058     def setup(self, context):
00059         self.name = 'NAOqi Dashboard (%s)'%rosenv.get_master_uri()
00060 
00061         self._robot_combobox = AvahiWidget()
00062 
00063         # Diagnostics
00064         self._monitor = MonitorDashWidget(self.context)
00065 
00066         # Rosout
00067         self._console = ConsoleDashWidget(self.context, minimal=False)
00068 
00069         ## Joint temperature
00070         self._temp_joint_button = StatusControl('Joint temperature', 'temperature_joints')
00071 
00072         ## CPU temperature
00073         self._temp_head_button = StatusControl('CPU temperature', 'temperature_head')
00074 
00075         ## Motors
00076         self._motors_button = Motors(self.context)
00077 
00078         ## Battery State
00079         self._power_state_ctrl = PowerStateControl('Battery')
00080 
00081         self._agg_sub = rospy.Subscriber('diagnostics_agg', DiagnosticArray, self.new_diagnostic_message)
00082 
00083     def get_widgets(self):
00084         return [ [self._robot_combobox], 
00085                 [self._monitor, self._console, self._temp_joint_button, self._temp_head_button,
00086                  self._motors_button],
00087                 [self._power_state_ctrl]
00088                 ]
00089 
00090     def shutdown_dashboard(self):
00091         self._agg_sub.unregister()
00092 
00093     def new_diagnostic_message(self, msg):
00094         """
00095         callback to process dashboard_agg messages
00096 
00097         :param msg: dashboard_agg DashboardState message
00098         :type msg: pr2_msgs.msg.DashboardState
00099         """
00100         self._dashboard_message = msg
00101         for status in msg.status:
00102             if status.name == '/Joints':
00103                 highestTemp = ""
00104                 lowestStiff = -1.0
00105                 highestStiff = -1.0
00106                 hotJoints = ""
00107                 for kv in status.values:
00108                      if kv.key == 'Highest Temperature':
00109                          highestTemp = kv.value
00110                      elif kv.key == 'Highest Stiffness':
00111                          highestStiff = float(kv.value)
00112                      elif kv.key == 'Lowest Stiffness without Hands':
00113                          lowestStiff = float(kv.value)
00114                      elif kv.key == 'Hot Joints':
00115                          hotJoints = str(kv.value)
00116                 self.set_buttonStatus(self._temp_joint_button, status, "Joints ", " (%s deg C  %s)"%(highestTemp, hotJoints))
00117                 # deal with the stiffness button
00118                 if(lowestStiff < 0.0 or highestStiff < 0.0):
00119                     self._motors_button.set_stale()
00120                 elif(lowestStiff > 0.9):
00121                     self._motors_button.set_error()
00122                 elif(highestStiff < 0.05):
00123                     self._motors_button.set_ok()
00124                 else:
00125                     self._motors_button.set_warn()
00126                 self.set_buttonStatus(self._motors_button, status, "Stiffness ", " (low: %f / high: %f)"%(lowestStiff, highestStiff))
00127             elif status.name == '/Computer':
00128                 for kv in status.values:
00129                     if kv.key == "Temperature":
00130                         self.set_buttonStatus(self._temp_head_button, status, "CPU temperature ", "(%s C)" % kv.value)
00131             elif status.name == '/Power System/Battery':
00132                 if status.level == 3:
00133                     self._power_state_ctrl.set_stale()
00134                 else:
00135                     self._power_state_ctrl.set_power_state(status.values)
00136 
00137     def set_buttonStatus(self, button, status, statusPrefix = "", statusSuffix = ""):
00138         statusString = "Unknown"
00139         if status.level == DiagnosticStatus.OK:
00140             button.update_state(0)
00141             statusString = "OK"
00142         elif status.level == DiagnosticStatus.WARN:
00143             button.update_state(1)
00144             statusString = "Warn"
00145         elif status.level == DiagnosticStatus.ERROR:
00146             button.update_state(2)
00147             statusString = "Error"
00148         elif status.level == 3:
00149             button.update_state(3)
00150             statusString = "Stale"
00151         button.setToolTip(statusPrefix + statusString + statusSuffix)
00152 
00153     def save_settings(self, plugin_settings, instance_settings):
00154         self._console.save_settings(plugin_settings, instance_settings)
00155         self._monitor.save_settings(plugin_settings, instance_settings)
00156 
00157     def restore_settings(self, plugin_settings, instance_settings):
00158         self._console.restore_settings(plugin_settings, instance_settings)
00159         self._monitor.restore_settings(plugin_settings, instance_settings)


nao_dashboard
Author(s): Stefan Osswald
autogenerated on Thu Aug 6 2015 17:26:05