_diagnostic_updater.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2012, 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 Willow Garage, Inc. 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 # -*- coding: utf-8 -*-
00034 
00035 """ diagnostic_updater for Python.
00036 @author Brice Rebsamen <brice [dot] rebsamen [gmail]>
00037 """
00038 
00039 import roslib
00040 roslib.load_manifest('diagnostic_updater')
00041 import rospy
00042 import threading, httplib
00043 from diagnostic_msgs.msg import DiagnosticArray
00044 
00045 from ._diagnostic_status_wrapper import *
00046 
00047 class DiagnosticTask:
00048     """DiagnosticTask is an abstract base class for collecting diagnostic data.
00049 
00050     Subclasses are provided for generating common diagnostic information.
00051     A DiagnosticTask has a name, and a function that is called to cleate a
00052     DiagnosticStatusWrapper.
00053     """
00054 
00055     def __init__(self, name):
00056         """Constructs a DiagnosticTask setting its name in the process."""
00057         self.name = name
00058 
00059     def getName(self):
00060         """Returns the name of the DiagnosticTask."""
00061         return self.name
00062 
00063     def run(self, stat):
00064         """Fills out this Task's DiagnosticStatusWrapper.
00065         @param stat: the DiagnosticStatusWrapper to fill
00066         @return the filled DiagnosticStatusWrapper
00067         """
00068         return stat
00069 
00070 
00071 
00072 class FunctionDiagnosticTask(DiagnosticTask):
00073     """A DiagnosticTask based on a function.
00074 
00075     The FunctionDiagnosticTask calls the function when it updates. The
00076     function updates the DiagnosticStatusWrapper and collects data.
00077 
00078     This is useful for gathering information about a device or driver, like temperature,
00079     calibration, etc.
00080     """
00081 
00082     def __init__(self, name, fn):
00083         """Constructs a GenericFunctionDiagnosticTask based on the given name and function.
00084         @param name Name of the function.
00085         @param fn Function to be called when run is called.
00086         """
00087         DiagnosticTask.__init__(self, name)
00088         self.fn = fn
00089 
00090     def run(self, stat):
00091         return self.fn(stat)
00092 
00093 
00094 
00095 class CompositeDiagnosticTask(DiagnosticTask):
00096     """Merges CompositeDiagnosticTask into a single DiagnosticTask.
00097 
00098     The CompositeDiagnosticTask allows multiple DiagnosticTask instances to
00099     be combined into a single task that produces a single single
00100     DiagnosticStatusWrapped. The output of the combination has the max of
00101     the status levels, and a concatenation of the non-zero-level messages.
00102 
00103     For instance, this could be used to combine the calibration and offset data
00104     from an IMU driver.
00105     """
00106 
00107     def __init__(self, name):
00108         """Constructs a CompositeDiagnosticTask with the given name."""
00109         DiagnosticTask.__init__(self, name)
00110         self.tasks = []
00111 
00112     def run(self, stat):
00113         """Runs each child and merges their outputs."""
00114         combined_summary = DiagnosticStatusWrapper()
00115         original_summary = DiagnosticStatusWrapper()
00116 
00117         original_summary.summary(stat)
00118 
00119         for task in self.tasks:
00120             # Put the summary that was passed in.
00121             stat.summary(original_summary)
00122             # Let the next task add entries and put its summary.
00123             stat = task.run(stat)
00124             # Merge the new summary into the combined summary.
00125             combined_summary.mergeSummary(stat)
00126 
00127 
00128         # Copy the combined summary into the output.
00129         stat.summary(combined_summary)
00130         return stat
00131 
00132     def addTask(self, t):
00133         """Adds a child CompositeDiagnosticTask.
00134 
00135         This CompositeDiagnosticTask will be called each time this
00136         CompositeDiagnosticTask is run.
00137         """
00138         self.tasks.append(t)
00139 
00140 
00141 
00142 class DiagnosticTaskVector:
00143     """Internal use only.
00144 
00145     Base class for diagnostic_updater::Updater and self_test::Dispatcher.
00146     The class manages a collection of diagnostic updaters. It contains the
00147     common functionality used for producing diagnostic updates and for
00148     self-tests.
00149     """
00150 
00151     class DiagnosticTaskInternal:
00152         """Class used to represent a diagnostic task internally in
00153         DiagnosticTaskVector.
00154         """
00155 
00156         def __init__(self, name, fn):
00157             self.name = name
00158             self.fn = fn
00159 
00160         def run(self, stat):
00161             stat.name = self.name
00162             return self.fn(stat)
00163 
00164 
00165     def __init__(self):
00166         self.tasks = []
00167         self.lock = threading.Lock()
00168 
00169     def addedTaskCallback(self, task):
00170         """Allows an action to be taken when a task is added. The Updater class
00171         uses this to immediately publish a diagnostic that says that the node
00172         is loading.
00173         """
00174         pass
00175 
00176     def add(self, *args):
00177         """Add a task to the DiagnosticTaskVector.
00178 
00179         Usage:
00180         add(task): where task is a DiagnosticTask
00181         add(name, fn): add a DiagnosticTask embodied by a name and function
00182         """
00183         if len(args)==1:
00184             task = DiagnosticTaskVector.DiagnosticTaskInternal(args[0].getName(), args[0].run)
00185         elif len(args)==2:
00186             task = DiagnosticTaskVector.DiagnosticTaskInternal(args[0], args[1])
00187 
00188         with self.lock:
00189             self.tasks.append(task)
00190             self.addedTaskCallback(task)
00191 
00192     def removeByName(self, name):
00193         """Removes a task based on its name.
00194 
00195         Removes the first task that matches the specified name. (New in
00196         version 1.1.2)
00197 
00198         @param name Name of the task to remove.
00199         @return Returns true if a task matched and was removed.
00200         """
00201         found = False
00202         with self.lock:
00203             for i in range(len(self.tasks)):
00204                 if self.tasks[i].name == name:
00205                     self.tasks.pop(i)
00206                     found = True
00207                     break
00208         return found
00209 
00210 
00211 
00212 
00213 class Updater(DiagnosticTaskVector):
00214     """Manages a list of diagnostic tasks, and calls them in a rate-limited manner.
00215 
00216     This class manages a list of diagnostic tasks. Its update function
00217     should be called frequently. At some predetermined rate, the update
00218     function will cause all the diagnostic tasks to run, and will collate
00219     and publish the resulting diagnostics. The publication rate is
00220     determined by the "~diagnostic_period" ros parameter.
00221 
00222     The class also allows an update to be forced when something significant
00223     has happened, and allows a single message to be broadcast on all the
00224     diagnostics if normal operation of the node is suspended for some
00225     reason.
00226     """
00227 
00228     def __init__(self):
00229         """Constructs an updater class."""
00230         DiagnosticTaskVector.__init__(self)
00231         self.publisher = rospy.Publisher("/diagnostics", DiagnosticArray)
00232 
00233         self.last_time = rospy.Time.now()
00234         self.last_time_period_checked = self.last_time
00235         self.period = 1
00236 
00237         self.verbose = False
00238         self.hwid = ""
00239         self.warn_nohwid_done = False
00240 
00241     def update(self):
00242         """Causes the diagnostics to update if the inter-update interval
00243         has been exceeded.
00244         """
00245         self._check_diagnostic_period()
00246         if rospy.Time.now() >= self.last_time + rospy.Duration(self.period):
00247             self.force_update()
00248 
00249     def force_update(self):
00250         """Forces the diagnostics to update.
00251 
00252         Useful if the node has undergone a drastic state change that should be
00253         published immediately.
00254         """
00255         self.last_time = rospy.Time.now()
00256 
00257         warn_nohwid = len(self.hwid)==0
00258 
00259         status_vec = []
00260 
00261         with self.lock: # Make sure no adds happen while we are processing here.
00262             for task in self.tasks:
00263                 status = DiagnosticStatusWrapper()
00264                 status.name = task.name
00265                 status.level = 2
00266                 status.message = "No message was set"
00267                 status.hardware_id = self.hwid
00268 
00269                 stat = task.run(status)
00270 
00271                 status_vec.append(status)
00272 
00273                 if status.level:
00274                     warn_nohwid = False
00275 
00276                 if self.verbose and status.level:
00277                     rospy.logwarn("Non-zero diagnostic status. Name: '%s', status %i: '%s'" %
00278                                 (status.name, status.level, status.message))
00279 
00280         if warn_nohwid and not self.warn_nohwid_done:
00281             rospy.logwarn("diagnostic_updater: No HW_ID was set. This is probably a bug. Please report it. For devices that do not have a HW_ID, set this value to 'none'. This warning only occurs once all diagnostics are OK so it is okay to wait until the device is open before calling setHardwareID.");
00282             self.warn_nohwid_done = True
00283 
00284         self.publish(status_vec)
00285 
00286     def broadcast(self, lvl, msg):
00287         """Outputs a message on all the known DiagnosticStatus.
00288 
00289         Useful if something drastic is happening such as shutdown or a self-test.
00290 
00291         @param lvl Level of the diagnostic being output.
00292         @param msg Status message to output.
00293         """
00294 
00295         status_vec = []
00296 
00297         for task in self.tasks:
00298             status = DiagnosticStatusWrapper()
00299             status.name = task.name
00300             status.summary(lvl, msg)
00301             status_vec.append(status)
00302 
00303         self.publish(status_vec)
00304 
00305     def setHardwareID(self, hwid):
00306         self.hwid = hwid
00307 
00308     def _check_diagnostic_period(self):
00309         """Recheck the diagnostic_period on the parameter server."""
00310 
00311         # This was getParamCached() call in the cpp code. i.e. it would throttle
00312         # the actual call to the parameter server using a notification of change
00313         # mechanism.
00314         # This is not available in rospy. Hence I throttle the call to the
00315         # parameter server using a standard timeout mechanism (4Hz)
00316 
00317         now = rospy.Time.now()
00318         if  now >= self.last_time_period_checked + rospy.Duration(0.25):
00319             try:
00320                 self.period = rospy.get_param("~diagnostic_period", 1)
00321                 self.last_time_period_checked = now
00322             except (httplib.CannotSendRequest, httplib.ResponseNotReady):
00323                 pass
00324 
00325     def publish(self, msg):
00326         """Publishes a single diagnostic status or a vector of diagnostic statuses."""
00327         if not type(msg) is list:
00328             msg = [msg]
00329 
00330         for stat in msg:
00331             stat.name = rospy.get_name()[1:]+ ": " + stat.name
00332 
00333         da = DiagnosticArray()
00334         da.status = msg
00335         da.header.stamp = rospy.Time.now() # Add timestamp for ROS 0.10
00336         self.publisher.publish(da)
00337 
00338     def addedTaskCallback(self, task):
00339         stat = DiagnosticStatusWrapper()
00340         stat.name = task.name
00341         stat.summary(0, "Node starting up")
00342         self.publish(stat)


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Fri Aug 28 2015 10:29:57