rosdiagnostic.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2017, Clearpath Inc.
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 Willow Garage, Inc. 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 import argparse
00034 import os
00035 import re
00036 import rosgraph
00037 import rospy
00038 import socket
00039 import sys
00040 
00041 from datetime import datetime
00042 from dateutil.tz import tzlocal
00043 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
00044 
00045 
00046 class ROSTopicException(Exception):
00047     """
00048     Base exception class of related errors
00049     """
00050     pass
00051 
00052 
00053 class ROSTopicIOException(ROSTopicException):
00054     """
00055     Errors related to network I/O failures
00056     """
00057     pass
00058 
00059 
00060 def _check_master():
00061     """
00062     Make sure that master is available
00063     :raises: :exc:`ROSTopicException` If unable to successfully communicate with master
00064     """
00065     try:
00066         rosgraph.Master('/rostopic').getPid()
00067     except socket.error:
00068         raise ROSTopicIOException("Unable to communicate with master!")
00069 
00070 
00071 class RosDiagnostics(object):
00072 
00073     DIAG_MSG_LVL_STR_NOCOLOR = {
00074         DiagnosticStatus.OK:    ' OK  ',
00075         DiagnosticStatus.WARN:  'WARN ',
00076         DiagnosticStatus.ERROR: 'ERROR',
00077         DiagnosticStatus.STALE: 'STALE',
00078     }
00079 
00080     DIAG_MSG_LVL_STR_COLOR = {
00081         DiagnosticStatus.OK:    '\033[92m OK  \033[0m',
00082         DiagnosticStatus.WARN:  '\033[93mWARN \033[0m',
00083         DiagnosticStatus.ERROR: '\033[91mERROR\033[0m',
00084         DiagnosticStatus.STALE: '\033[94mSTALE\033[0m',
00085     }
00086 
00087     def __init__(self, options):
00088         self._lvl_strings_map = self.DIAG_MSG_LVL_STR_NOCOLOR if options.nocolor else self.DIAG_MSG_LVL_STR_COLOR
00089         self._options = options
00090         rospy.Subscriber(options.topic, DiagnosticArray, self._diag_callback, queue_size=1)
00091 
00092     def _get_ns(self, name):
00093         # Split namespace by slashes, return the base namespace.
00094         return '/'.join(name.split('/')[:-1])
00095 
00096     def _get_non_leaf_statuses(self, statuses):
00097         return [self._get_ns(s.name) for s in statuses]
00098 
00099     def _get_leaf_statuses(self, statuses):
00100         parent_namespaces = self._get_non_leaf_statuses(statuses)
00101         return [s for s in statuses if s.name not in parent_namespaces]
00102 
00103     def _print_begin_banner(self, msg):
00104         if not self._options.follow:
00105             dt = datetime.fromtimestamp(msg.header.stamp.to_sec(), tzlocal())
00106             print('=====================================================================')
00107             print('Diagnostics generated on: {}'.format(dt))
00108             print('---------------------------------------------------------------------')
00109 
00110     def _print_end_banner(self, msg):
00111         if not self._options.follow:
00112             print('=====================================================================')
00113             rospy.signal_shutdown('Run complete')
00114 
00115     def _diag_callback(self, msg):
00116         prog = re.compile(self._options.filter)
00117         self._print_begin_banner(msg)
00118         for diag in sorted(self._get_leaf_statuses(msg.status), key=lambda d: d.level, reverse=True):
00119             if prog.search(diag.name):
00120                 self._print_status(diag, msg.header.stamp)
00121         self._print_end_banner(msg)
00122 
00123     def _print_status(self, diag, ts):
00124         if self._options.min_level <= diag.level:
00125             print('[ {} ] {} - {}'.format(self._lvl_strings_map[diag.level], diag.name, diag.message))
00126             if self._options.detail:
00127                 dt = datetime.fromtimestamp(ts.to_sec(), tzlocal())
00128                 print('    timestamp:   {}'.format(dt))
00129                 print('    hardware_id: {}'.format(diag.hardware_id))
00130                 for kv in diag.values:
00131                     print('    - {}: {}'.format(kv.key, kv.value))
00132 
00133 
00134 def _rosdiagnostic_cmd_echo(argv):
00135     parser = argparse.ArgumentParser(
00136         description='ROS Diagnostic Viewer is a command-line tool for printing information about ROS Diagnostics.'
00137     )
00138     parser.add_argument('-f', '--follow', action='store_true', dest='follow',
00139                         help='follows the diagnostic messages continuously')
00140 
00141     parser.add_argument('-l', '--level', action='store', metavar='LEVEL', type=int, default=DiagnosticStatus.WARN,
00142                         dest='min_level',
00143                         help='the minimum diagnostic level to display (OK=0, WARN=1, ERROR=2, STALE=3) [default=1]')
00144 
00145     parser.add_argument('--topic', action='store', metavar='TOPIC', type=str, default='/diagnostics_agg', dest='topic',
00146                         help='topic to read the diagnostics from')
00147 
00148     parser.add_argument('--filter', action='store', metavar='FILTER', type=str, default='.*', dest='filter',
00149                         help='regular expression to be applied as a filter to the diagnostic name')
00150 
00151     parser.add_argument('--nocolor', action='store_true',
00152                         help='output should not make use of any color')
00153 
00154     parser.add_argument('-d', '--detail', action='store_true',
00155                         help='printing the full diagnostic details ')
00156 
00157     args = parser.parse_args(argv)
00158 
00159     _check_master()
00160     rospy.init_node('rosdiagnostic', anonymous=True)
00161     rosdiag = RosDiagnostics(args)
00162     rospy.spin()
00163     del rosdiag
00164 
00165 
00166 def rosdiagnosticmain(argv=None):
00167     if argv is None:
00168         argv = sys.argv
00169     # filter out remapping arguments in case we are being invoked via roslaunch
00170     argv = rospy.myargv(argv)
00171 
00172     try:
00173         _rosdiagnostic_cmd_echo(argv[1:])
00174     except socket.error:
00175         sys.stderr.write("Network communication failed. Most likely failed to communicate with master.\n")
00176         sys.exit(1)
00177     except rosgraph.MasterException as e:
00178         # mainly for invalid master URI/rosgraph.masterapi
00179         sys.stderr.write("ERROR: %s\n" % str(e))
00180         sys.exit(1)
00181     except ROSTopicException as e:
00182         sys.stderr.write("ERROR: %s\n" % str(e))
00183         sys.exit(1)
00184     except KeyboardInterrupt:
00185         pass
00186     except rospy.ROSInterruptException:
00187         pass


rosdiagnostic
Author(s): Guillaume Autran
autogenerated on Tue Mar 26 2019 03:09:46